From e083269bd15ddb7442785e3dafafc08e9dacfb34 Mon Sep 17 00:00:00 2001 From: aayli Date: Tue, 11 Apr 2023 16:22:36 +0200 Subject: [PATCH 1/7] thread lock --- panther_battery/src/adc_node.py | 121 ++++----- .../src/roboteq_republisher_node.py | 3 +- panther_driver/src/driver_node.py | 225 ++++++++-------- panther_lights/src/controller_node.py | 114 +++++---- panther_lights/src/scheduler_node.py | 141 +++++----- panther_manager/src/manager_node.py | 240 +++++++++--------- panther_power_control/src/power_board_node.py | 43 ++-- panther_power_control/src/relays_node.py | 18 +- 8 files changed, 473 insertions(+), 432 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 8339b6795..0afe12841 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -1,5 +1,7 @@ #!/usr/bin/python3 +from threading import Lock + import math import rospy @@ -15,6 +17,8 @@ class ADCNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + self._V_driv_front = None self._V_driv_rear = None self._I_driv_front = None @@ -59,72 +63,75 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _motor_controllers_state_cb(self, driver_state: DriverState) -> None: - self._V_driv_front = driver_state.front.voltage - self._V_driv_rear = driver_state.front.current - self._I_driv_front = driver_state.rear.voltage - self._I_driv_rear = driver_state.rear.current + with self._lock: + self._V_driv_front = driver_state.front.voltage + self._V_driv_rear = driver_state.front.current + self._I_driv_front = driver_state.rear.voltage + self._I_driv_rear = driver_state.rear.current def _io_state_cb(self, io_state: IOState) -> None: - self._charger_connected = io_state.charger_connected + with self._lock: + self._charger_connected = io_state.charger_connected def _battery_timer_cb(self, *args) -> None: - try: - V_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage0_raw', LSB=0.02504255, offset=0.0 - ) - V_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage3_raw', LSB=0.02504255, offset=0.0 - ) - V_temp_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage1_raw', LSB=0.002, offset=0.0 - ) - V_temp_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage0_raw', LSB=0.002, offset=0.0 - ) - I_charge_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage3_raw', LSB=0.005, offset=0.0 - ) - I_charge_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage2_raw', LSB=0.005, offset=0.0 - ) - I_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage2_raw', LSB=0.04, offset=625.0 - ) - I_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage1_raw', LSB=0.04, offset=625.0 - ) - except: - rospy.logerr(f'[{rospy.get_name()}] Battery ADC measurement error') - return + with self._lock: + try: + V_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage0_raw', LSB=0.02504255, offset=0.0 + ) + V_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage3_raw', LSB=0.02504255, offset=0.0 + ) + V_temp_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage1_raw', LSB=0.002, offset=0.0 + ) + V_temp_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage0_raw', LSB=0.002, offset=0.0 + ) + I_charge_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage3_raw', LSB=0.005, offset=0.0 + ) + I_charge_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage2_raw', LSB=0.005, offset=0.0 + ) + I_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage2_raw', LSB=0.04, offset=625.0 + ) + I_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage1_raw', LSB=0.04, offset=625.0 + ) + except: + rospy.logerr(f'[{rospy.get_name()}] Battery ADC measurement error') + return - if self._battery_count == 2: - temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) - temp_bat_2 = self._voltage_to_deg(V_temp_bat_2) + if self._battery_count == 2: + temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) + temp_bat_2 = self._voltage_to_deg(V_temp_bat_2) - self._publish_battery_msg( - self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1 - ) - self._publish_battery_msg( - self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2 - ) + self._publish_battery_msg( + self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1 + ) + self._publish_battery_msg( + self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2 + ) - V_bat_avereage = (V_bat_1 + V_bat_2) / 2.0 - temp_bat_average = (temp_bat_1 + temp_bat_2) / 2.0 - I_bat_sum = I_bat_1 + I_bat_2 - I_charge_bat = I_charge_bat_1 + I_charge_bat_2 + V_bat_avereage = (V_bat_1 + V_bat_2) / 2.0 + temp_bat_average = (temp_bat_1 + temp_bat_2) / 2.0 + I_bat_sum = I_bat_1 + I_bat_2 + I_charge_bat = I_charge_bat_1 + I_charge_bat_2 - self._publish_battery_msg( - self._battery_pub, - V_bat_avereage, - temp_bat_average, - -I_bat_sum + I_charge_bat, - ) + self._publish_battery_msg( + self._battery_pub, + V_bat_avereage, + temp_bat_average, + -I_bat_sum + I_charge_bat, + ) - else: - temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) - self._publish_battery_msg( - self._battery_pub, V_bat_1, temp_bat_1, -(I_bat_1 + I_bat_2) + I_charge_bat_1 - ) + else: + temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) + self._publish_battery_msg( + self._battery_pub, V_bat_1, temp_bat_1, -(I_bat_1 + I_bat_2) + I_charge_bat_1 + ) def _check_battery_count(self) -> int: trials_num = 10 diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 0f272982f..23ad3532a 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -13,11 +13,12 @@ class RoboteqRepublisherNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + self._battery_voltage = None self._battery_current = None self._battery_timeout = 1.0 self._last_battery_info_time = rospy.get_time() - self._lock = Lock() # ------------------------------- # Subscribers diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index aef920109..4bd866cf1 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -2,6 +2,7 @@ import math from typing import List, Tuple, TypeVar +from threading import Lock import rospy from tf.transformations import quaternion_from_euler @@ -66,6 +67,8 @@ class PantherDriverNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + self._eds_file = rospy.get_param('~eds_file') self._use_pdo = rospy.get_param('~use_pdo', False) self._can_interface = rospy.get_param('~can_interface', 'panther_can') @@ -258,120 +261,126 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _main_timer_cb(self, *args) -> None: - time_now = rospy.Time.now() - dt = (time_now - self._time_last).to_sec() - self._time_last = time_now - - if (time_now - self._cmd_vel_command_last_time) < rospy.Duration( - secs=self._cmd_vel_timeout - ): - self._panther_can.write_wheels_enc_velocity(self._panther_kinematics.wheels_enc_speed) - else: - self._panther_can.write_wheels_enc_velocity([0.0, 0.0, 0.0, 0.0]) - - wheel_enc_pos = self._panther_can.query_wheels_enc_pose() - wheel_enc_vel = self._panther_can.query_wheels_enc_velocity() - wheel_enc_curr = self._panther_can.query_motor_current() - - # convert tics to rad - self._wheels_ang_pos = [ - (2.0 * math.pi) * (pos / (self._encoder_resolution * self._gear_ratio)) - for pos in wheel_enc_pos - ] - # convert RPM to rad/s - self._wheels_ang_vel = [ - (2.0 * math.pi / 60.0) * (vel / self._gear_ratio) for vel in wheel_enc_vel - ] - # convert A to Nm - self._motors_effort = [ - enc_curr * self._motor_torque_constant for enc_curr in wheel_enc_curr - ] - - try: - self._robot_pos, self._robot_vel = self._panther_kinematics.forward_kinematics( - *self._wheels_ang_vel, dt=dt - ) - except: - rospy.logwarn(f'[{rospy.get_name()}] Could not get robot pose') - - self._robot_orientation_quat = quaternion_from_euler(0.0, 0.0, self._robot_pos[2]) - - if self._publish_joints: - self._publish_joint_state_cb() - if self._publish_pose: - self._publish_pose_cb() - if self._publish_odom: - self._publish_odom_cb() - if self._publish_tf: - self._publish_tf_cb() + with self._lock: + time_now = rospy.Time.now() + dt = (time_now - self._time_last).to_sec() + self._time_last = time_now + + if (time_now - self._cmd_vel_command_last_time) < rospy.Duration( + secs=self._cmd_vel_timeout + ): + self._panther_can.write_wheels_enc_velocity(self._panther_kinematics.wheels_enc_speed) + else: + self._panther_can.write_wheels_enc_velocity([0.0, 0.0, 0.0, 0.0]) + + wheel_enc_pos = self._panther_can.query_wheels_enc_pose() + wheel_enc_vel = self._panther_can.query_wheels_enc_velocity() + wheel_enc_curr = self._panther_can.query_motor_current() + + # convert tics to rad + self._wheels_ang_pos = [ + (2.0 * math.pi) * (pos / (self._encoder_resolution * self._gear_ratio)) + for pos in wheel_enc_pos + ] + # convert RPM to rad/s + self._wheels_ang_vel = [ + (2.0 * math.pi / 60.0) * (vel / self._gear_ratio) for vel in wheel_enc_vel + ] + # convert A to Nm + self._motors_effort = [ + enc_curr * self._motor_torque_constant for enc_curr in wheel_enc_curr + ] + + try: + self._robot_pos, self._robot_vel = self._panther_kinematics.forward_kinematics( + *self._wheels_ang_vel, dt=dt + ) + except: + rospy.logwarn(f'[{rospy.get_name()}] Could not get robot pose') + + self._robot_orientation_quat = quaternion_from_euler(0.0, 0.0, self._robot_pos[2]) + + if self._publish_joints: + self._publish_joint_state() + if self._publish_pose: + self._publish_pose() + if self._publish_odom: + self._publish_odom() + if self._publish_tf: + self._publish_tf() def _driver_state_timer_cb(self, *args) -> None: - [ - self._driver_state_msg.front.current, - self._driver_state_msg.front.voltage, - self._driver_state_msg.rear.current, - self._driver_state_msg.rear.voltage, - ] = self._panther_can.query_battery_data() - - [ - self._driver_state_msg.front.temperature, - self._driver_state_msg.rear.temperature, - ] = self._panther_can.query_driver_temperature_data() - - [self._driver_state_msg.front.fault_flag, self._driver_state_msg.rear.fault_flag] = [ - self._driver_flag_loggers['fault_flags'](flag_val) - for flag_val in self._panther_can.query_fault_flags() - ] - - ( - self._driver_state_msg.front.fault_flag.can_net_err, - self._driver_state_msg.rear.fault_flag.can_net_err, - ) = self._panther_can.can_connection_error() - - [self._driver_state_msg.front.script_flag, self._driver_state_msg.rear.script_flag] = [ - self._driver_flag_loggers['script_flags'](flag_val) - for flag_val in self._panther_can.query_script_flags() - ] - - [ - self._driver_state_msg.front.right_motor.runtime_error, - self._driver_state_msg.front.left_motor.runtime_error, - self._driver_state_msg.rear.right_motor.runtime_error, - self._driver_state_msg.rear.left_motor.runtime_error, - ] = [ - self._driver_flag_loggers['runtime_errors'](flag_val) - for flag_val in self._panther_can.query_runtime_stat_flag() - ] - - self._driver_state_pub.publish(self._driver_state_msg) + with self._lock: + [ + self._driver_state_msg.front.current, + self._driver_state_msg.front.voltage, + self._driver_state_msg.rear.current, + self._driver_state_msg.rear.voltage, + ] = self._panther_can.query_battery_data() + + [ + self._driver_state_msg.front.temperature, + self._driver_state_msg.rear.temperature, + ] = self._panther_can.query_driver_temperature_data() + + [self._driver_state_msg.front.fault_flag, self._driver_state_msg.rear.fault_flag] = [ + self._driver_flag_loggers['fault_flags'](flag_val) + for flag_val in self._panther_can.query_fault_flags() + ] + + ( + self._driver_state_msg.front.fault_flag.can_net_err, + self._driver_state_msg.rear.fault_flag.can_net_err, + ) = self._panther_can.can_connection_error() + + [self._driver_state_msg.front.script_flag, self._driver_state_msg.rear.script_flag] = [ + self._driver_flag_loggers['script_flags'](flag_val) + for flag_val in self._panther_can.query_script_flags() + ] + + [ + self._driver_state_msg.front.right_motor.runtime_error, + self._driver_state_msg.front.left_motor.runtime_error, + self._driver_state_msg.rear.right_motor.runtime_error, + self._driver_state_msg.rear.left_motor.runtime_error, + ] = [ + self._driver_flag_loggers['runtime_errors'](flag_val) + for flag_val in self._panther_can.query_runtime_stat_flag() + ] + + self._driver_state_pub.publish(self._driver_state_msg) def _safety_timer_cb(self, *args) -> None: - if any(self._panther_can.can_connection_error()): - if not self._e_stop_cliented: - self._trigger_panther_e_stop() + with self._lock: + if any(self._panther_can.can_connection_error()): + if not self._e_stop_cliented: + self._trigger_panther_e_stop() + self._stop_cmd_vel_cb = True + + rospy.logerr_throttle( + 10.0, + f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure). ' + f'Please ensure that controllers are powered on.', + ) + elif self._e_stop_cliented: self._stop_cmd_vel_cb = True - - rospy.logerr_throttle( - 10.0, - f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure). ' - f'Please ensure that controllers are powered on.', - ) - elif self._e_stop_cliented: - self._stop_cmd_vel_cb = True - else: - self._stop_cmd_vel_cb = False + else: + self._stop_cmd_vel_cb = False def _e_stop_cb(self, data: Bool) -> None: - self._e_stop_cliented = data.data + with self._lock: + self._e_stop_cliented = data.data def _cmd_vel_cb(self, data: Twist) -> None: - # Block all motors if any Roboteq controller returns a fault flag or runtime error flag - if not self._stop_cmd_vel_cb: - self._panther_kinematics.inverse_kinematics(data) - else: - self._panther_kinematics.inverse_kinematics(Twist()) + with self._lock: - self._cmd_vel_command_last_time = rospy.Time.now() + # Block all motors if any Roboteq controller returns a fault flag or runtime error flag + if not self._stop_cmd_vel_cb: + self._panther_kinematics.inverse_kinematics(data) + else: + self._panther_kinematics.inverse_kinematics(Twist()) + + self._cmd_vel_command_last_time = rospy.Time.now() def _reset_roboteq_script_cb(self, req: TriggerRequest) -> TriggerResponse: try: @@ -382,7 +391,7 @@ def _reset_roboteq_script_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(False, f'Roboteq script reset failed') - def _publish_joint_state_cb(self) -> None: + def _publish_joint_state(self) -> None: self._joint_state_msg.header.stamp = rospy.Time.now() self._joint_state_msg.position = [ math.atan2(math.sin(pos), math.cos(pos)) for pos in self._wheels_ang_pos @@ -391,7 +400,7 @@ def _publish_joint_state_cb(self) -> None: self._joint_state_msg.effort = self._motors_effort self._joint_state_pub.publish(self._joint_state_msg) - def _publish_pose_cb(self) -> None: + def _publish_pose(self) -> None: self._pose_msg.position.x = self._robot_pos[0] self._pose_msg.position.y = self._robot_pos[1] self._pose_msg.orientation.x = self._robot_orientation_quat[0] @@ -400,7 +409,7 @@ def _publish_pose_cb(self) -> None: self._pose_msg.orientation.w = self._robot_orientation_quat[3] self._pose_pub.publish(self._pose_msg) - def _publish_tf_cb(self) -> None: + def _publish_tf(self) -> None: self._tf_stamped.header.stamp = rospy.Time.now() self._tf_stamped.transform.translation.x = self._robot_pos[0] self._tf_stamped.transform.translation.y = self._robot_pos[1] @@ -411,7 +420,7 @@ def _publish_tf_cb(self) -> None: self._tf_stamped.transform.rotation.w = self._robot_orientation_quat[3] self._tf_broadcaster.sendTransform(self._tf_stamped) - def _publish_odom_cb(self) -> None: + def _publish_odom(self) -> None: self._odom_msg.header.stamp = rospy.Time.now() self._odom_msg.pose.pose.position.x = self._robot_pos[0] self._odom_msg.pose.pose.position.y = self._robot_pos[1] diff --git a/panther_lights/src/controller_node.py b/panther_lights/src/controller_node.py index badaeda30..dd10519a9 100755 --- a/panther_lights/src/controller_node.py +++ b/panther_lights/src/controller_node.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 from copy import deepcopy +from threading import Lock import rospy @@ -17,7 +18,6 @@ from animation import Animation, BASIC_ANIMATIONS - class PantherAnimation: ANIMATION_DEFAULT_PRIORITY: int = 3 ANIMATION_DEFAULT_TIMEOUT: float = 120.0 @@ -105,6 +105,8 @@ class LightsControllerNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + # check for required ROS parameters if not rospy.has_param('~animations'): rospy.logerr( @@ -174,65 +176,67 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _controller_timer_cb(self, *args) -> None: - brightness_front = 255 - brightness_rear = 255 - frame_front = self._empty_frame - frame_rear = self._empty_frame - - if self._animation_finished: - self._anim_queue.validate_queue() + with self._lock: + brightness_front = 255 + brightness_rear = 255 + frame_front = self._empty_frame + frame_rear = self._empty_frame - if self._default_animation and not self._anim_queue.has_animation( - self._default_animation - ): - self._default_animation.reset_time() - self._anim_queue.put(self._default_animation) - - if not self._anim_queue.empty(): - self._current_animation = self._anim_queue.get() + if self._animation_finished: + self._anim_queue.validate_queue() + + if self._default_animation and not self._anim_queue.has_animation( + self._default_animation + ): + self._default_animation.reset_time() + self._anim_queue.put(self._default_animation) + + if not self._anim_queue.empty(): + self._current_animation = self._anim_queue.get() + + if self._current_animation: + if self._current_animation.priority > self._anim_queue.first_anim_priority: + if self._current_animation.repeating: + self._current_animation.front.reset() + self._current_animation.rear.reset() + elif self._current_animation.front.progress < 0.65: + self._current_animation.front.reset() + self._current_animation.rear.reset() + self._anim_queue.put(self._current_animation) + self._animation_finished = True + self._current_animation = None + return + + if not self._current_animation.front.finished: + frame_front = self._current_animation.front() + brightness_front = self._current_animation.front.brightness + if not self._current_animation.rear.finished: + frame_rear = self._current_animation.rear() + brightness_rear = self._current_animation.rear.brightness + self._animation_finished = ( + self._current_animation.front.finished and self._current_animation.rear.finished + ) - if self._current_animation: - if self._current_animation.priority > self._anim_queue.first_anim_priority: - if self._current_animation.repeating: - self._current_animation.front.reset() - self._current_animation.rear.reset() - elif self._current_animation.front.progress < 0.65: + if self._animation_finished: self._current_animation.front.reset() self._current_animation.rear.reset() - self._anim_queue.put(self._current_animation) - self._animation_finished = True - self._current_animation = None - return - - if not self._current_animation.front.finished: - frame_front = self._current_animation.front() - brightness_front = self._current_animation.front.brightness - if not self._current_animation.rear.finished: - frame_rear = self._current_animation.rear() - brightness_rear = self._current_animation.rear.brightness - self._animation_finished = ( - self._current_animation.front.finished and self._current_animation.rear.finished - ) - - if self._animation_finished: - self._current_animation.front.reset() - self._current_animation.rear.reset() - self._current_animation = None + self._current_animation = None - self._front_frame_pub.publish( - self._rgb_frame_to_img_msg(frame_front, brightness_front, 'front_light_link') - ) - self._rear_frame_pub.publish( - self._rgb_frame_to_img_msg(frame_rear, brightness_rear, 'rear_light_link') - ) + self._front_frame_pub.publish( + self._rgb_frame_to_img_msg(frame_front, brightness_front, 'front_light_link') + ) + self._rear_frame_pub.publish( + self._rgb_frame_to_img_msg(frame_rear, brightness_rear, 'rear_light_link') + ) def _animation_queue_timer_cb(self, *args) -> None: - anim_queue_msg = LEDAnimationQueue() - if not self._anim_queue.empty(): - anim_queue_msg.queue = [anim.name for anim in self._anim_queue.queue] - if self._current_animation: - anim_queue_msg.queue.insert(0, self._current_animation.name) - self._animation_queue_pub.publish(anim_queue_msg) + with self._lock: + anim_queue_msg = LEDAnimationQueue() + if not self._anim_queue.empty(): + anim_queue_msg.queue = [anim.name for anim in self._anim_queue.queue] + if self._current_animation: + anim_queue_msg.queue.insert(0, self._current_animation.name) + self._animation_queue_pub.publish(anim_queue_msg) def _set_animation_cb(self, req: SetLEDAnimationRequest) -> SetLEDAnimationResponse: if not req.animation.id in self._animations: @@ -252,9 +256,7 @@ def _set_animation_cb(self, req: SetLEDAnimationRequest) -> SetLEDAnimationRespo True, f'Successfully set an animation with id {req.animation.id}' ) - def _set_image_animation_cb( - self, req: SetLEDImageAnimationRequest - ) -> SetLEDImageAnimationResponse: + def _set_image_animation_cb(self, req: SetLEDImageAnimationRequest) -> SetLEDImageAnimationResponse: animation = PantherAnimation() animation.repeating = req.repeating diff --git a/panther_lights/src/scheduler_node.py b/panther_lights/src/scheduler_node.py index 1974f14fc..d3c242f8d 100755 --- a/panther_lights/src/scheduler_node.py +++ b/panther_lights/src/scheduler_node.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import rospy +from threading import Lock from sensor_msgs.msg import BatteryState from std_msgs.msg import Bool @@ -13,6 +14,8 @@ class LightsSchedulerNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + self._battery_percentage = 1.0 self._battery_status = None self._charging_percentage = -1.0 # -1.0 to trigger animation when charger gets connected @@ -26,21 +29,17 @@ def __init__(self, name: str) -> None: ] self._critical_battery_anim_period = rospy.get_param('~critical_battery_anim_period', 15.0) - self._critical_battery_threshold_percent = rospy.get_param( - '~critical_battery_threshold_percent', 0.1 - ) + self._critical_battery_threshold_percent = rospy.get_param('~critical_battery_threshold_percent', 0.1) self._battery_state_anim_period = rospy.get_param('~battery_state_anim_period', 120.0) self._low_battery_anim_period = rospy.get_param('~low_battery_anim_period', 30.0) self._low_battery_threshold_percent = rospy.get_param('~low_battery_threshold_percent', 0.4) self._update_charging_anim_step = rospy.get_param('~update_charging_anim_step', 0.1) # ------------------------------- - # Publishers + # Subscribers # ------------------------------- - self._battery_sub = rospy.Subscriber( - 'battery', BatteryState, self._battery_cb, queue_size=1 - ) + self._battery_sub = rospy.Subscriber('battery', BatteryState, self._battery_cb, queue_size=1) self._e_stop_sub = rospy.Subscriber('hardware/e_stop', Bool, self._e_stop_cb, queue_size=1) # ------------------------------- @@ -78,40 +77,41 @@ def __init__(self, name: str) -> None: def _scheduler_timer_cb(self, *args) -> None: # call animation service only when e_stop state changes - if ( - self._led_e_stop_state != self._e_stop_state - and not self._battery_status in self._charger_connected_states - ): - req = SetLEDAnimationRequest() - req.repeating = True - if self._e_stop_state: - req.animation.id = LEDAnimation.E_STOP - success = self._call_led_animation_srv(req) - else: - req.animation.id = LEDAnimation.READY - success = self._call_led_animation_srv(req) - if success: - self._led_e_stop_state = self._e_stop_state - - if ( - self._battery_status in self._charger_connected_states - and not self._charging_battery_timer - ): - self._charging_battery_timer_cb() # manually trigger timers callback - self._charging_battery_timer = rospy.Timer( - rospy.Duration(6.0), - self._charging_battery_timer_cb, - ) - elif ( - not self._battery_status in self._charger_connected_states - and self._charging_battery_timer - ): - self._charging_battery_timer.shutdown() - self._charging_battery_timer.join() - del self._charging_battery_timer - self._charging_battery_timer = None - self._charging_percentage = -1.0 - self._led_e_stop_state = None + with self._lock: + if ( + self._led_e_stop_state != self._e_stop_state + and not self._battery_status in self._charger_connected_states + ): + req = SetLEDAnimationRequest() + req.repeating = True + if self._e_stop_state: + req.animation.id = LEDAnimation.E_STOP + success = self._call_led_animation_srv(req) + else: + req.animation.id = LEDAnimation.READY + success = self._call_led_animation_srv(req) + if success: + self._led_e_stop_state = self._e_stop_state + + if ( + self._battery_status in self._charger_connected_states + and not self._charging_battery_timer + ): + self._charging_battery_timer_cb() # manually trigger timers callback + self._charging_battery_timer = rospy.Timer( + rospy.Duration(6.0), + self._charging_battery_timer_cb, + ) + elif ( + not self._battery_status in self._charger_connected_states + and self._charging_battery_timer + ): + self._charging_battery_timer.shutdown() + self._charging_battery_timer.join() + del self._charging_battery_timer + self._charging_battery_timer = None + self._charging_percentage = -1.0 + self._led_e_stop_state = None def _charging_battery_timer_cb(self, *args) -> None: if self._e_stop_state: @@ -134,41 +134,46 @@ def _charging_battery_timer_cb(self, *args) -> None: self._call_led_animation_srv(req) def _critical_battery_timer_cb(self, *args) -> None: - if ( - self._battery_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING - and self._battery_percentage < self._critical_battery_threshold_percent - ): - req = SetLEDAnimationRequest() - req.animation.id = LEDAnimation.CRITICAL_BATTERY - self._call_led_animation_srv(req) + with self._lock: + if ( + self._battery_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + and self._battery_percentage < self._critical_battery_threshold_percent + ): + req = SetLEDAnimationRequest() + req.animation.id = LEDAnimation.CRITICAL_BATTERY + self._call_led_animation_srv(req) def _battery_state_timer_cb(self, *args) -> None: - if self._battery_status in [ - BatteryState.POWER_SUPPLY_STATUS_DISCHARGING, - BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING, - ]: - req = SetLEDAnimationRequest() - req.animation.id = LEDAnimation.BATTERY_STATE - req.animation.param = str(self._battery_percentage) - self._call_led_animation_srv(req) + with self._lock: + if self._battery_status in [ + BatteryState.POWER_SUPPLY_STATUS_DISCHARGING, + BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING, + ]: + req = SetLEDAnimationRequest() + req.animation.id = LEDAnimation.BATTERY_STATE + req.animation.param = str(self._battery_percentage) + self._call_led_animation_srv(req) def _low_battery_timer_cb(self, *args) -> None: - if ( - self._battery_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING - and self._critical_battery_threshold_percent - <= self._battery_percentage - < self._low_battery_threshold_percent - ): - req = SetLEDAnimationRequest() - req.animation.id = LEDAnimation.LOW_BATTERY - self._call_led_animation_srv(req) + with self._lock: + if ( + self._battery_status == BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + and self._critical_battery_threshold_percent + <= self._battery_percentage + < self._low_battery_threshold_percent + ): + req = SetLEDAnimationRequest() + req.animation.id = LEDAnimation.LOW_BATTERY + self._call_led_animation_srv(req) def _battery_cb(self, battery_state: BatteryState) -> None: - self._battery_percentage = battery_state.percentage - self._battery_status = battery_state.power_supply_status + with self._lock: + self._battery_percentage = battery_state.percentage + self._battery_status = battery_state.power_supply_status def _e_stop_cb(self, e_stop_state: Bool) -> None: - self._e_stop_state = e_stop_state.data + with self._lock: + self._e_stop_state = e_stop_state.data def _call_led_animation_srv(self, req: SetLEDAnimationRequest) -> bool: try: diff --git a/panther_manager/src/manager_node.py b/panther_manager/src/manager_node.py index 4e777de50..5a99943c6 100755 --- a/panther_manager/src/manager_node.py +++ b/panther_manager/src/manager_node.py @@ -1,5 +1,7 @@ #!/usr/bin/python3 +from threading import Lock + import os import paramiko import socket @@ -17,6 +19,7 @@ class ManagerNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() self._aux_power_state = None self._e_stop_state = None self._fan_state = None @@ -105,22 +108,16 @@ def __init__(self, name: str) -> None: # ------------------------------- self._battery_sub = rospy.Subscriber('battery', BatteryState, self._battery_cb) - self._driver_state_sub = rospy.Subscriber( - 'driver/motor_controllers_state', DriverState, self._driver_state_cb - ) + self._driver_state_sub = rospy.Subscriber('driver/motor_controllers_state', DriverState, self._driver_state_cb) self._e_stop_sub = rospy.Subscriber('hardware/e_stop', Bool, self._e_stop_cb) self._io_state_sub = rospy.Subscriber('hardware/io_state', IOState, self._io_state_cb) - self._system_status_sub = rospy.Subscriber( - 'system_status', SystemStatus, self._system_status_cb - ) + self._system_status_sub = rospy.Subscriber('system_status', SystemStatus, self._system_status_cb) # ------------------------------- # Service servers # ------------------------------- - self._overwrite_fan_control_server = rospy.Service( - 'manager/overwrite_fan_control', SetBool, self._overwrite_fan_control_cb - ) + self._overwrite_fan_control_server = rospy.Service('manager/overwrite_fan_control', SetBool, self._overwrite_fan_control_cb) # ------------------------------- # Service clients @@ -142,43 +139,48 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _battery_cb(self, battery_state: BatteryState) -> None: - self._battery_status = battery_state.power_supply_status - if self._battery_temp_window is not None: - self._battery_temp_window = self._move_window( - self._battery_temp_window, battery_state.temperature - ) - else: - self._battery_temp_window = [battery_state.temperature] * self._battery_window_len + with self._lock: + self._battery_status = battery_state.power_supply_status + if self._battery_temp_window is not None: + self._battery_temp_window = self._move_window( + self._battery_temp_window, battery_state.temperature + ) + else: + self._battery_temp_window = [battery_state.temperature] * self._battery_window_len def _driver_state_cb(self, driver_state: DriverState) -> None: - if self._front_driver_temp_window is not None and self._rear_driver_temp_window is not None: - self._front_driver_temp_window = self._move_window( - self._front_driver_temp_window, driver_state.front.temperature - ) - self._rear_driver_temp_window = self._move_window( - self._rear_driver_temp_window, driver_state.rear.temperature - ) - else: - self._front_driver_temp_window = [ - driver_state.front.temperature - ] * self._driver_window_len - self._rear_driver_temp_window = [ - driver_state.rear.temperature - ] * self._driver_window_len + with self._lock: + if self._front_driver_temp_window is not None and self._rear_driver_temp_window is not None: + self._front_driver_temp_window = self._move_window( + self._front_driver_temp_window, driver_state.front.temperature + ) + self._rear_driver_temp_window = self._move_window( + self._rear_driver_temp_window, driver_state.rear.temperature + ) + else: + self._front_driver_temp_window = [ + driver_state.front.temperature + ] * self._driver_window_len + self._rear_driver_temp_window = [ + driver_state.rear.temperature + ] * self._driver_window_len def _e_stop_cb(self, e_stop_state: Bool) -> None: - self._e_stop_state = e_stop_state.data + with self._lock: + self._e_stop_state = e_stop_state.data def _io_state_cb(self, io_state: IOState) -> None: - self._aux_power_state = io_state.aux_power - self._fan_state = io_state.fan - self._power_button_state = io_state.power_button + with self._lock: + self._aux_power_state = io_state.aux_power + self._fan_state = io_state.fan + self._power_button_state = io_state.power_button def _system_status_cb(self, system_status: SystemStatus) -> None: - if self._cpu_temp_window is not None: - self._cpu_temp_window = self._move_window(self._cpu_temp_window, system_status.cpu_temp) - else: - self._cpu_temp_window = [system_status.cpu_temp] * self._cpu_window_len + with self._lock: + if self._cpu_temp_window is not None: + self._cpu_temp_window = self._move_window(self._cpu_temp_window, system_status.cpu_temp) + else: + self._cpu_temp_window = [system_status.cpu_temp] * self._cpu_window_len def _overwrite_fan_control_cb(self, req: SetBoolRequest) -> SetBoolResponse: self._overwrite_fan_control = req.data @@ -187,40 +189,41 @@ def _overwrite_fan_control_cb(self, req: SetBoolRequest) -> SetBoolResponse: return SetBoolResponse(True, f'Overwrite fan control set to: {req.data}') def _manager_timer_cb(self, *args) -> None: - if self._power_button_state: - rospy.loginfo(f'[{rospy.get_name()}] Power button pressed, shutting down robot') - self._shutdown() - return - - if self._battery_temp_window is None: - rospy.loginfo_throttle( - 5.0, f'[{rospy.get_name()}] Waiting for battery message to arrive.' - ) - return + with self._lock: + if self._power_button_state: + rospy.loginfo(f'[{rospy.get_name()}] Power button pressed, shutting down robot') + self._shutdown() + return + + if self._battery_temp_window is None: + rospy.loginfo_throttle( + 5.0, f'[{rospy.get_name()}] Waiting for battery message to arrive.' + ) + return - if self._battery_status == BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING: - rospy.logwarn_throttle(5.0, f'[{rospy.get_name()}] Battery is not charging.') + if self._battery_status == BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING: + rospy.logwarn_throttle(5.0, f'[{rospy.get_name()}] Battery is not charging.') - self._battery_avg_temp = self._get_mean(self._battery_temp_window) - if self._battery_avg_temp > self._fatal_bat_temp: - rospy.logerr_throttle( - 5.0, f'[{rospy.get_name()}] Fatal battery temperature, shutting down robot!' - ) - self._shutdown() - elif self._battery_avg_temp > self._high_bat_temp: - if self._battery_avg_temp > self._critical_bat_temp: - rospy.logerr_throttle( - 5.0, - f'[{rospy.get_name()}] Critical battery temperature, triggering E-STOP and disabling AUX!', - ) - if self._aux_power_state: - self._call_set_bool_service(self._aux_power_enable_client, False) - else: + self._battery_avg_temp = self._get_mean(self._battery_temp_window) + if self._battery_avg_temp > self._fatal_bat_temp: rospy.logerr_throttle( - 5.0, f'[{rospy.get_name()}] High battery temperature, triggering E-STOP!' + 5.0, f'[{rospy.get_name()}] Fatal battery temperature, shutting down robot!' ) - if not self._e_stop_state: - self._call_trigger_service(self._e_stop_trigger_client) + self._shutdown() + elif self._battery_avg_temp > self._high_bat_temp: + if self._battery_avg_temp > self._critical_bat_temp: + rospy.logerr_throttle( + 5.0, + f'[{rospy.get_name()}] Critical battery temperature, triggering E-STOP and disabling AUX!', + ) + if self._aux_power_state: + self._call_set_bool_service(self._aux_power_enable_client, False) + else: + rospy.logerr_throttle( + 5.0, f'[{rospy.get_name()}] High battery temperature, triggering E-STOP!' + ) + if not self._e_stop_state: + self._call_trigger_service(self._e_stop_trigger_client) def _shutdown(self) -> None: rospy.logwarn(f'[{rospy.get_name()}] Soft shutdown initialized.') @@ -278,62 +281,63 @@ def _check_ip(self, host: str) -> bool: return os.system('ping -c 1 -w 1 ' + host + ' > /dev/null') == 0 def _fan_control_timer_cb(self, *args) -> None: - if self._fan_state is None: - rospy.loginfo(f'[{rospy.get_name()}] Waiting for fan state message to arrive.') - return + with self._lock: + if self._fan_state is None: + rospy.loginfo(f'[{rospy.get_name()}] Waiting for fan state message to arrive.') + return - if self._overwrite_fan_control: - return + if self._overwrite_fan_control: + return - if self._cpu_temp_window is None: - rospy.loginfo(f'[{rospy.get_name()}] Waiting for system_status message to arrive.') - return + if self._cpu_temp_window is None: + rospy.loginfo(f'[{rospy.get_name()}] Waiting for system_status message to arrive.') + return - if self._front_driver_temp_window is None or self._rear_driver_temp_window is None: - rospy.loginfo( - f'[{rospy.get_name()}] Waiting for motor_controllers_state message to arrive.' - ) - return + if self._front_driver_temp_window is None or self._rear_driver_temp_window is None: + rospy.loginfo( + f'[{rospy.get_name()}] Waiting for motor_controllers_state message to arrive.' + ) + return - self._cpu_avg_temp = self._get_mean(self._cpu_temp_window) - self._front_driver_avg_temp = self._get_mean(self._front_driver_temp_window) - self._rear_driver_avg_temp = self._get_mean(self._rear_driver_temp_window) + self._cpu_avg_temp = self._get_mean(self._cpu_temp_window) + self._front_driver_avg_temp = self._get_mean(self._front_driver_temp_window) + self._rear_driver_avg_temp = self._get_mean(self._rear_driver_temp_window) - if self._front_driver_avg_temp > self._critical_driver_temp: - rospy.logerr_throttle( - 60, - f'[{rospy.get_name()}] Front driver reached critical ', - f'temperature of {int(round(self._front_driver_avg_temp) + 0.1)} deg C!', - ) - if self._rear_driver_avg_temp > self._critical_driver_temp: - rospy.logerr_throttle( - 60, - f'[{rospy.get_name()}] Rear driver reached critical ', - f'temperature of {int(round(self._rear_driver_avg_temp) + 0.1)} deg C!', - ) + if self._front_driver_avg_temp > self._critical_driver_temp: + rospy.logerr_throttle( + 60, + f'[{rospy.get_name()}] Front driver reached critical ', + f'temperature of {int(round(self._front_driver_avg_temp) + 0.1)} deg C!', + ) + if self._rear_driver_avg_temp > self._critical_driver_temp: + rospy.logerr_throttle( + 60, + f'[{rospy.get_name()}] Rear driver reached critical ', + f'temperature of {int(round(self._rear_driver_avg_temp) + 0.1)} deg C!', + ) - if not self._fan_state and ( - self._cpu_avg_temp > self._cpu_fan_on_temp - or self._front_driver_avg_temp > self._driver_fan_on_temp - or self._rear_driver_avg_temp > self._driver_fan_on_temp - ): - rospy.loginfo(f'[{rospy.get_name()}] Turning on fan. Cooling the robot.') - self._turn_on_time = rospy.Time.now() - self._call_set_bool_service(self._fan_enable_client, True) - return - - if ( - self._fan_state - and (rospy.Time.now() - self._turn_on_time).secs > self._fun_enable_hysteresis - and ( - self._cpu_avg_temp < self._cpu_fan_off_temp - and self._front_driver_avg_temp < self._driver_fan_off_temp - and self._rear_driver_avg_temp < self._driver_fan_off_temp - ) - ): - rospy.loginfo(f'[{rospy.get_name()}] Turning off fan.') - self._call_set_bool_service(self._fan_enable_client, False) - return + if not self._fan_state and ( + self._cpu_avg_temp > self._cpu_fan_on_temp + or self._front_driver_avg_temp > self._driver_fan_on_temp + or self._rear_driver_avg_temp > self._driver_fan_on_temp + ): + rospy.loginfo(f'[{rospy.get_name()}] Turning on fan. Cooling the robot.') + self._turn_on_time = rospy.Time.now() + self._call_set_bool_service(self._fan_enable_client, True) + return + + if ( + self._fan_state + and (rospy.Time.now() - self._turn_on_time).secs > self._fun_enable_hysteresis + and ( + self._cpu_avg_temp < self._cpu_fan_off_temp + and self._front_driver_avg_temp < self._driver_fan_off_temp + and self._rear_driver_avg_temp < self._driver_fan_off_temp + ) + ): + rospy.loginfo(f'[{rospy.get_name()}] Turning off fan.') + self._call_set_bool_service(self._fan_enable_client, False) + return def _move_window(self, window: list, elem: float) -> list: window = window[1:] diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 2e26d5c49..ecc41c9fa 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -2,6 +2,7 @@ from dataclasses import dataclass import RPi.GPIO as GPIO +from threading import Lock import rospy @@ -58,6 +59,7 @@ class PowerBoardNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() self._clearing_e_stop = False self._pins = PatherGPIO() @@ -146,35 +148,40 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _cmd_vel_cb(self, *args) -> None: - self._cmd_vel_msg_time = rospy.get_time() + with self._lock: + self._cmd_vel_msg_time = rospy.get_time() def _motor_controllers_state_cb(self, msg: DriverState) -> None: - self._can_net_err = any({msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err}) + with self._lock: + self._can_net_err = any({msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err}) def _gpio_interrupt_cb(self, pin: int) -> None: - if pin == self._pins.SHDN_INIT: - self._chrg_sense_interrupt_time = rospy.get_time() + with self._lock: + if pin == self._pins.SHDN_INIT: + self._chrg_sense_interrupt_time = rospy.get_time() - if pin == self._pins.E_STOP_RESET: - self._e_stop_interrupt_time = rospy.get_time() + if pin == self._pins.E_STOP_RESET: + self._e_stop_interrupt_time = rospy.get_time() def _publish_pin_state_cb(self, *args) -> None: - charger_state = self._read_pin(self._pins.CHRG_SENSE) - self._publish_io_state('charger_connected', charger_state) + with self._lock: + charger_state = self._read_pin(self._pins.CHRG_SENSE) + self._publish_io_state('charger_connected', charger_state) - # filter short spikes of voltage on GPIO - if rospy.get_time() - self._chrg_sense_interrupt_time > self._gpio_wait: - if self._read_pin(self._pins.SHDN_INIT): - rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') - self._publish_io_state('power_button', True) - self._chrg_sense_interrupt_time = float('inf') + # filter short spikes of voltage on GPIO + if rospy.get_time() - self._chrg_sense_interrupt_time > self._gpio_wait: + if self._read_pin(self._pins.SHDN_INIT): + rospy.loginfo(f'[{rospy.get_name()}] Shutdown button pressed.') + self._publish_io_state('power_button', True) + self._chrg_sense_interrupt_time = float('inf') - if rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: - self._e_stop_event() - self._e_stop_interrupt_time = float('inf') + if rospy.get_time() - self._e_stop_interrupt_time > self._gpio_wait: + self._e_stop_event() + self._e_stop_interrupt_time = float('inf') def _watchdog_cb(self, *args) -> None: - self._watchdog() + with self._lock: + self._watchdog() def _aux_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: res = self._set_bool_srv_handle(req.data, self._pins.AUX_PW_EN, 'Aux power enable') diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index b3b4d3940..f1d20765f 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -2,6 +2,7 @@ from dataclasses import dataclass import RPi.GPIO as GPIO +from threading import Lock import rospy @@ -25,6 +26,8 @@ class RelaysNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + self._pins = PatherGPIO() self._setup_gpio() @@ -73,10 +76,12 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _cmd_vel_cb(self, *args) -> None: - self._cmd_vel_msg_time = rospy.get_time() + with self._lock: + self._cmd_vel_msg_time = rospy.get_time() def _motor_controllers_state_cb(self, msg: DriverState) -> None: - self._can_net_err = any({msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err}) + with self._lock: + self._can_net_err = any({msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err}) def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: if rospy.get_time() - self._cmd_vel_msg_time <= 2.0: @@ -100,10 +105,11 @@ def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(True, 'E-SROP triggered successful') def _set_motor_state_timer_cb(self, *args) -> None: - motor_state = GPIO.input(self._pins.STAGE2_INPUT) - GPIO.output(self._pins.MOTOR_ON, motor_state) - if motor_state != self._motor_on_pub.impl.latch.data: - self._motor_on_pub.publish(motor_state) + with self._lock: + motor_state = GPIO.input(self._pins.STAGE2_INPUT) + GPIO.output(self._pins.MOTOR_ON, motor_state) + if motor_state != self._motor_on_pub.impl.latch.data: + self._motor_on_pub.publish(motor_state) def _setup_gpio(self) -> None: GPIO.setwarnings(False) From f264f9d8c0a325c7d902220b2405b75500bd0f15 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 12 Apr 2023 11:39:32 +0200 Subject: [PATCH 2/7] undo fcn naming --- panther_driver/src/driver_node.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 4bd866cf1..f380fc5bd 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -301,13 +301,13 @@ def _main_timer_cb(self, *args) -> None: self._robot_orientation_quat = quaternion_from_euler(0.0, 0.0, self._robot_pos[2]) if self._publish_joints: - self._publish_joint_state() + self._publish_joint_state_cb() if self._publish_pose: - self._publish_pose() + self._publish_pose_cb() if self._publish_odom: - self._publish_odom() + self._publish_odom_cb() if self._publish_tf: - self._publish_tf() + self._publish_tf_cb() def _driver_state_timer_cb(self, *args) -> None: with self._lock: @@ -391,7 +391,7 @@ def _reset_roboteq_script_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(False, f'Roboteq script reset failed') - def _publish_joint_state(self) -> None: + def _publish_joint_state_cb(self) -> None: self._joint_state_msg.header.stamp = rospy.Time.now() self._joint_state_msg.position = [ math.atan2(math.sin(pos), math.cos(pos)) for pos in self._wheels_ang_pos @@ -400,7 +400,7 @@ def _publish_joint_state(self) -> None: self._joint_state_msg.effort = self._motors_effort self._joint_state_pub.publish(self._joint_state_msg) - def _publish_pose(self) -> None: + def _publish_pose_cb(self) -> None: self._pose_msg.position.x = self._robot_pos[0] self._pose_msg.position.y = self._robot_pos[1] self._pose_msg.orientation.x = self._robot_orientation_quat[0] @@ -409,7 +409,7 @@ def _publish_pose(self) -> None: self._pose_msg.orientation.w = self._robot_orientation_quat[3] self._pose_pub.publish(self._pose_msg) - def _publish_tf(self) -> None: + def _publish_tf_cb(self) -> None: self._tf_stamped.header.stamp = rospy.Time.now() self._tf_stamped.transform.translation.x = self._robot_pos[0] self._tf_stamped.transform.translation.y = self._robot_pos[1] @@ -420,7 +420,7 @@ def _publish_tf(self) -> None: self._tf_stamped.transform.rotation.w = self._robot_orientation_quat[3] self._tf_broadcaster.sendTransform(self._tf_stamped) - def _publish_odom(self) -> None: + def _publish_odom_cb(self) -> None: self._odom_msg.header.stamp = rospy.Time.now() self._odom_msg.pose.pose.position.x = self._robot_pos[0] self._odom_msg.pose.pose.position.y = self._robot_pos[1] From 29210f192af2cd0855587aa441d821ff9e743a4c Mon Sep 17 00:00:00 2001 From: aayli Date: Thu, 13 Apr 2023 11:04:44 +0200 Subject: [PATCH 3/7] few more lock --- panther_driver/src/driver_node.py | 14 +-- panther_lights/src/controller_node.py | 75 ++++++++-------- panther_lights/src/driver_node.py | 82 ++++++++--------- panther_manager/src/manager_node.py | 9 +- panther_power_control/src/power_board_node.py | 88 ++++++++++--------- panther_power_control/src/relays_node.py | 36 ++++---- 6 files changed, 159 insertions(+), 145 deletions(-) diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index f380fc5bd..072d96a3f 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -373,7 +373,6 @@ def _e_stop_cb(self, data: Bool) -> None: def _cmd_vel_cb(self, data: Twist) -> None: with self._lock: - # Block all motors if any Roboteq controller returns a fault flag or runtime error flag if not self._stop_cmd_vel_cb: self._panther_kinematics.inverse_kinematics(data) @@ -383,13 +382,14 @@ def _cmd_vel_cb(self, data: Twist) -> None: self._cmd_vel_command_last_time = rospy.Time.now() def _reset_roboteq_script_cb(self, req: TriggerRequest) -> TriggerResponse: - try: - if self._panther_can.restart_roboteq_script(): - return TriggerResponse(True, 'Roboteq script reset successful.') - except Exception as err: - return TriggerResponse(False, f'Roboteq script reset failed: \n{err}') + with self._lock: + try: + if self._panther_can.restart_roboteq_script(): + return TriggerResponse(True, 'Roboteq script reset successful.') + except Exception as err: + return TriggerResponse(False, f'Roboteq script reset failed: \n{err}') - return TriggerResponse(False, f'Roboteq script reset failed') + return TriggerResponse(False, f'Roboteq script reset failed') def _publish_joint_state_cb(self) -> None: self._joint_state_msg.header.stamp = rospy.Time.now() diff --git a/panther_lights/src/controller_node.py b/panther_lights/src/controller_node.py index dd10519a9..78b113e3c 100755 --- a/panther_lights/src/controller_node.py +++ b/panther_lights/src/controller_node.py @@ -239,50 +239,53 @@ def _animation_queue_timer_cb(self, *args) -> None: self._animation_queue_pub.publish(anim_queue_msg) def _set_animation_cb(self, req: SetLEDAnimationRequest) -> SetLEDAnimationResponse: - if not req.animation.id in self._animations: - return SetLEDAnimationResponse(False, f'No Animation with id: {req.animation.id}') - - try: - animation = deepcopy(self._animations[req.animation.id]) - animation.front.set_param(req.animation.param) - animation.rear.set_param(req.animation.param) - animation.reset_time() - animation.repeating = req.repeating - self._add_animation_to_queue(animation) - except ValueError as err: - return SetLEDAnimationResponse(False, f'Failed to add animation to queue: {err}') - - return SetLEDAnimationResponse( - True, f'Successfully set an animation with id {req.animation.id}' - ) + with self._lock: + if not req.animation.id in self._animations: + return SetLEDAnimationResponse(False, f'No Animation with id: {req.animation.id}') + + try: + animation = deepcopy(self._animations[req.animation.id]) + animation.front.set_param(req.animation.param) + animation.rear.set_param(req.animation.param) + animation.reset_time() + animation.repeating = req.repeating + self._add_animation_to_queue(animation) + except ValueError as err: + return SetLEDAnimationResponse(False, f'Failed to add animation to queue: {err}') + + return SetLEDAnimationResponse( + True, f'Successfully set an animation with id {req.animation.id}' + ) def _set_image_animation_cb(self, req: SetLEDImageAnimationRequest) -> SetLEDImageAnimationResponse: - animation = PantherAnimation() - animation.repeating = req.repeating + with self._lock: + animation = PantherAnimation() + animation.repeating = req.repeating - try: - animation_description_front = self._get_image_animation_description(req.front) - animation_description_rear = self._get_image_animation_description(req.rear) + try: + animation_description_front = self._get_image_animation_description(req.front) + animation_description_rear = self._get_image_animation_description(req.rear) - animation.front = BASIC_ANIMATIONS['image_animation']( - animation_description_front, self._num_led, self._controller_frequency - ) - animation.rear = BASIC_ANIMATIONS['image_animation']( - animation_description_rear, self._num_led, self._controller_frequency - ) + animation.front = BASIC_ANIMATIONS['image_animation']( + animation_description_front, self._num_led, self._controller_frequency + ) + animation.rear = BASIC_ANIMATIONS['image_animation']( + animation_description_rear, self._num_led, self._controller_frequency + ) - self._add_animation_to_queue(animation) - except Exception as err: - return SetLEDImageAnimationResponse(False, f'{err}') + self._add_animation_to_queue(animation) + except Exception as err: + return SetLEDImageAnimationResponse(False, f'{err}') - return SetLEDImageAnimationResponse(True, f'Successfully set custom animation') + return SetLEDImageAnimationResponse(True, f'Successfully set custom animation') def _update_animations_cb(self, req: TriggerRequest) -> TriggerResponse: - try: - self._update_user_animations() - except Exception as err: - return TriggerResponse(False, f'Failed to update animations: {err}') - return TriggerResponse(True, 'Animations updated successfully') + with self._lock: + try: + self._update_user_animations() + except Exception as err: + return TriggerResponse(False, f'Failed to update animations: {err}') + return TriggerResponse(True, 'Animations updated successfully') def _rgb_frame_to_img_msg(self, rgb_frame: list, brightness: int, frame_id: str) -> Image: img_msg = Image() diff --git a/panther_lights/src/driver_node.py b/panther_lights/src/driver_node.py index 5169ff5e4..5e04422a2 100755 --- a/panther_lights/src/driver_node.py +++ b/panther_lights/src/driver_node.py @@ -105,26 +105,28 @@ def _setup_panels(self) -> None: ) def _front_frame_cb(self, image: Image) -> None: - if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): - rospy.logwarn(f'[{rospy.get_name()}] Front frame timeout exceeded, ignoring frame.') - return - if image.header.stamp < self._last_time_stamp_front: - rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for front panel.') - return - self._last_time_stamp_front = image.header.stamp - rgb_frame, brightness = self._decode_img_msg(image) - self._set_panel_frame(LEDConstants.PANEL_FRONT, rgb_frame, brightness) + with self._lock: + if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): + rospy.logwarn(f'[{rospy.get_name()}] Front frame timeout exceeded, ignoring frame.') + return + if image.header.stamp < self._last_time_stamp_front: + rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for front panel.') + return + self._last_time_stamp_front = image.header.stamp + rgb_frame, brightness = self._decode_img_msg(image) + self._set_panel_frame(LEDConstants.PANEL_FRONT, rgb_frame, brightness) def _rear_frame_cb(self, image: Image) -> None: - if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): - rospy.logwarn(f'[{rospy.get_name()}] Rear frame timeout exceeded, ignoring frame.') - return - if image.header.stamp < self._last_time_stamp_rear: - rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for rear panel.') - return - self._last_time_stamp_rear = image.header.stamp - rgb_frame, brightness = self._decode_img_msg(image) - self._set_panel_frame(LEDConstants.PANEL_REAR, rgb_frame, brightness) + with self._lock: + if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): + rospy.logwarn(f'[{rospy.get_name()}] Rear frame timeout exceeded, ignoring frame.') + return + if image.header.stamp < self._last_time_stamp_rear: + rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for rear panel.') + return + self._last_time_stamp_rear = image.header.stamp + rgb_frame, brightness = self._decode_img_msg(image) + self._set_panel_frame(LEDConstants.PANEL_REAR, rgb_frame, brightness) def _decode_img_msg(self, image: Image) -> tuple: rgb_frame = [[image.data[i + j] for j in range(3)] for i in range(0, len(image.data), 4)] @@ -132,31 +134,31 @@ def _decode_img_msg(self, image: Image) -> tuple: return rgb_frame, brightness def _set_panel_frame(self, panel_num: int, panel_frame: list, brightness: int = 255) -> None: - with self._lock: - # select panel - if panel_num == LEDConstants.PANEL_FRONT and self._front_active: - GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_FRONT_STATE) - elif panel_num == LEDConstants.PANEL_REAR and self._rear_active: - GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_REAR_STATE) - else: - raise ValueError('panther lights have only two panels') - - for i, pixel in enumerate(panel_frame): - r = int(pixel[0] * self._color_correction[0]) - g = int(pixel[1] * self._color_correction[1]) - b = int(pixel[2] * self._color_correction[2]) - pixel_hex = (r << 16) + (g << 8) + b - self._pixels.set_pixel_rgb(i, pixel_hex, int(brightness / 255.0 * 100.0)) - self._pixels.show() + # select panel + if panel_num == LEDConstants.PANEL_FRONT and self._front_active: + GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_FRONT_STATE) + elif panel_num == LEDConstants.PANEL_REAR and self._rear_active: + GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_REAR_STATE) + else: + raise ValueError('panther lights have only two panels') + + for i, pixel in enumerate(panel_frame): + r = int(pixel[0] * self._color_correction[0]) + g = int(pixel[1] * self._color_correction[1]) + b = int(pixel[2] * self._color_correction[2]) + pixel_hex = (r << 16) + (g << 8) + b + self._pixels.set_pixel_rgb(i, pixel_hex, int(brightness / 255.0 * 100.0)) + self._pixels.show() def _set_brightness_cb(self, req: SetLEDBrightnessRequest) -> SetLEDBrightnessResponse: - try: - brightness = self._percent_to_apa_driver_brightness(req.data) - self._pixels.global_brightness = brightness - except ValueError as err: - return SetLEDBrightnessResponse(False, f'{err}') + with self._lock: + try: + brightness = self._percent_to_apa_driver_brightness(req.data) + self._pixels.global_brightness = brightness + except ValueError as err: + return SetLEDBrightnessResponse(False, f'{err}') - return SetLEDBrightnessResponse(True, f'Changed brightness to {req.data}') + return SetLEDBrightnessResponse(True, f'Changed brightness to {req.data}') def _clear_panel(self, panel_num: int) -> None: self._set_panel_frame(panel_num, [[0, 0, 0]] * self._num_led) diff --git a/panther_manager/src/manager_node.py b/panther_manager/src/manager_node.py index 5a99943c6..bac0a9429 100755 --- a/panther_manager/src/manager_node.py +++ b/panther_manager/src/manager_node.py @@ -183,10 +183,11 @@ def _system_status_cb(self, system_status: SystemStatus) -> None: self._cpu_temp_window = [system_status.cpu_temp] * self._cpu_window_len def _overwrite_fan_control_cb(self, req: SetBoolRequest) -> SetBoolResponse: - self._overwrite_fan_control = req.data - if req.data: - self._call_set_bool_service(self._fan_enable_client, req.data) - return SetBoolResponse(True, f'Overwrite fan control set to: {req.data}') + with self._lock: + self._overwrite_fan_control = req.data + if req.data: + self._call_set_bool_service(self._fan_enable_client, req.data) + return SetBoolResponse(True, f'Overwrite fan control set to: {req.data}') def _manager_timer_cb(self, *args) -> None: with self._lock: diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index ecc41c9fa..f553d2d1e 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -184,57 +184,63 @@ def _watchdog_cb(self, *args) -> None: self._watchdog() def _aux_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(req.data, self._pins.AUX_PW_EN, 'Aux power enable') - if res.success: - self._publish_io_state('aux_power', req.data) - return res + with self._lock: + res = self._set_bool_srv_handle(req.data, self._pins.AUX_PW_EN, 'Aux power enable') + if res.success: + self._publish_io_state('aux_power', req.data) + return res def _charger_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(not req.data, self._pins.CHRG_DISABLE, 'Charger disable') - if res.success: - self._publish_io_state('charger_enabled', req.data) - return res + with self._lock: + res = self._set_bool_srv_handle(not req.data, self._pins.CHRG_DISABLE, 'Charger disable') + if res.success: + self._publish_io_state('charger_enabled', req.data) + return res def _digital_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(req.data, self._pins.VDIG_OFF, 'Digital power enable') - if res.success: - self._publish_io_state('digital_power', req.data) - return res + with self._lock: + res = self._set_bool_srv_handle(req.data, self._pins.VDIG_OFF, 'Digital power enable') + if res.success: + self._publish_io_state('digital_power', req.data) + return res def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: - if self._validate_gpio_pin(self._pins.E_STOP_RESET, False): - return TriggerResponse(True, 'E-STOP is not active, reset is not needed') - elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: - return TriggerResponse( - False, - 'E-STOP reset failed, messages are still published on /cmd_vel topic!', - ) - elif self._can_net_err: - return TriggerResponse( - False, - 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', - ) - - self._reset_e_stop() - - if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): - self._watchdog.turn_off() - return TriggerResponse( - False, - 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', - ) - - return TriggerResponse(True, 'E-STOP reset successful') + with self._lock: + if self._validate_gpio_pin(self._pins.E_STOP_RESET, False): + return TriggerResponse(True, 'E-STOP is not active, reset is not needed') + elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: + return TriggerResponse( + False, + 'E-STOP reset failed, messages are still published on /cmd_vel topic!', + ) + elif self._can_net_err: + return TriggerResponse( + False, + 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', + ) + + self._reset_e_stop() + + if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): + self._watchdog.turn_off() + return TriggerResponse( + False, + 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', + ) + + return TriggerResponse(True, 'E-STOP reset successful') def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: - self._watchdog.turn_off() - return TriggerResponse(True, f'E-STOP triggered, watchdog turned off') + with self._lock: + self._watchdog.turn_off() + return TriggerResponse(True, f'E-STOP triggered, watchdog turned off') def _fan_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - res = self._set_bool_srv_handle(req.data, self._pins.FAN_SW, 'Fan enable') - if res.success: - self._publish_io_state('fan', req.data) - return res + with self._lock: + res = self._set_bool_srv_handle(req.data, self._pins.FAN_SW, 'Fan enable') + if res.success: + self._publish_io_state('fan', req.data) + return res def _set_bool_srv_handle(self, value: bool, pin: int, name: str) -> SetBoolResponse: rospy.logdebug(f'[{rospy.get_name()}] Requested {name} = {value}') diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index f1d20765f..5238df65e 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -84,25 +84,27 @@ def _motor_controllers_state_cb(self, msg: DriverState) -> None: self._can_net_err = any({msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err}) def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: - if rospy.get_time() - self._cmd_vel_msg_time <= 2.0: - return TriggerResponse( - False, - 'E-STOP reset failed, messages are still published on /cmd_vel topic!', - ) - elif self._can_net_err: - return TriggerResponse( - False, - 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', - ) - - self._e_stop_state = False - self._e_stop_state_pub.publish(self._e_stop_state) - return TriggerResponse(True, 'E-STOP reset successful') + with self._lock: + if rospy.get_time() - self._cmd_vel_msg_time <= 2.0: + return TriggerResponse( + False, + 'E-STOP reset failed, messages are still published on /cmd_vel topic!', + ) + elif self._can_net_err: + return TriggerResponse( + False, + 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', + ) + + self._e_stop_state = False + self._e_stop_state_pub.publish(self._e_stop_state) + return TriggerResponse(True, 'E-STOP reset successful') def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: - self._e_stop_state = True - self._e_stop_state_pub.publish(self._e_stop_state) - return TriggerResponse(True, 'E-SROP triggered successful') + with self._lock: + self._e_stop_state = True + self._e_stop_state_pub.publish(self._e_stop_state) + return TriggerResponse(True, 'E-SROP triggered successful') def _set_motor_state_timer_cb(self, *args) -> None: with self._lock: From 37319304dbb76bac243e2d541b75d5f5e6522c0b Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Wed, 19 Apr 2023 17:16:32 +0200 Subject: [PATCH 4/7] revert nodes --- panther_lights/src/driver_node.py | 82 +++++++++-------- panther_power_control/src/power_board_node.py | 88 +++++++++---------- 2 files changed, 81 insertions(+), 89 deletions(-) diff --git a/panther_lights/src/driver_node.py b/panther_lights/src/driver_node.py index 5e04422a2..5169ff5e4 100755 --- a/panther_lights/src/driver_node.py +++ b/panther_lights/src/driver_node.py @@ -105,28 +105,26 @@ def _setup_panels(self) -> None: ) def _front_frame_cb(self, image: Image) -> None: - with self._lock: - if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): - rospy.logwarn(f'[{rospy.get_name()}] Front frame timeout exceeded, ignoring frame.') - return - if image.header.stamp < self._last_time_stamp_front: - rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for front panel.') - return - self._last_time_stamp_front = image.header.stamp - rgb_frame, brightness = self._decode_img_msg(image) - self._set_panel_frame(LEDConstants.PANEL_FRONT, rgb_frame, brightness) + if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): + rospy.logwarn(f'[{rospy.get_name()}] Front frame timeout exceeded, ignoring frame.') + return + if image.header.stamp < self._last_time_stamp_front: + rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for front panel.') + return + self._last_time_stamp_front = image.header.stamp + rgb_frame, brightness = self._decode_img_msg(image) + self._set_panel_frame(LEDConstants.PANEL_FRONT, rgb_frame, brightness) def _rear_frame_cb(self, image: Image) -> None: - with self._lock: - if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): - rospy.logwarn(f'[{rospy.get_name()}] Rear frame timeout exceeded, ignoring frame.') - return - if image.header.stamp < self._last_time_stamp_rear: - rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for rear panel.') - return - self._last_time_stamp_rear = image.header.stamp - rgb_frame, brightness = self._decode_img_msg(image) - self._set_panel_frame(LEDConstants.PANEL_REAR, rgb_frame, brightness) + if (rospy.Time.now() - image.header.stamp) > rospy.Duration(self._frame_timeout): + rospy.logwarn(f'[{rospy.get_name()}] Rear frame timeout exceeded, ignoring frame.') + return + if image.header.stamp < self._last_time_stamp_rear: + rospy.logwarn(f'[{rospy.get_name()}] Dropping message from past for rear panel.') + return + self._last_time_stamp_rear = image.header.stamp + rgb_frame, brightness = self._decode_img_msg(image) + self._set_panel_frame(LEDConstants.PANEL_REAR, rgb_frame, brightness) def _decode_img_msg(self, image: Image) -> tuple: rgb_frame = [[image.data[i + j] for j in range(3)] for i in range(0, len(image.data), 4)] @@ -134,31 +132,31 @@ def _decode_img_msg(self, image: Image) -> tuple: return rgb_frame, brightness def _set_panel_frame(self, panel_num: int, panel_frame: list, brightness: int = 255) -> None: - # select panel - if panel_num == LEDConstants.PANEL_FRONT and self._front_active: - GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_FRONT_STATE) - elif panel_num == LEDConstants.PANEL_REAR and self._rear_active: - GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_REAR_STATE) - else: - raise ValueError('panther lights have only two panels') - - for i, pixel in enumerate(panel_frame): - r = int(pixel[0] * self._color_correction[0]) - g = int(pixel[1] * self._color_correction[1]) - b = int(pixel[2] * self._color_correction[2]) - pixel_hex = (r << 16) + (g << 8) + b - self._pixels.set_pixel_rgb(i, pixel_hex, int(brightness / 255.0 * 100.0)) - self._pixels.show() + with self._lock: + # select panel + if panel_num == LEDConstants.PANEL_FRONT and self._front_active: + GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_FRONT_STATE) + elif panel_num == LEDConstants.PANEL_REAR and self._rear_active: + GPIO.output(LEDConstants.LED_SWITCH_PIN, LEDConstants.LED_SWITCH_REAR_STATE) + else: + raise ValueError('panther lights have only two panels') + + for i, pixel in enumerate(panel_frame): + r = int(pixel[0] * self._color_correction[0]) + g = int(pixel[1] * self._color_correction[1]) + b = int(pixel[2] * self._color_correction[2]) + pixel_hex = (r << 16) + (g << 8) + b + self._pixels.set_pixel_rgb(i, pixel_hex, int(brightness / 255.0 * 100.0)) + self._pixels.show() def _set_brightness_cb(self, req: SetLEDBrightnessRequest) -> SetLEDBrightnessResponse: - with self._lock: - try: - brightness = self._percent_to_apa_driver_brightness(req.data) - self._pixels.global_brightness = brightness - except ValueError as err: - return SetLEDBrightnessResponse(False, f'{err}') + try: + brightness = self._percent_to_apa_driver_brightness(req.data) + self._pixels.global_brightness = brightness + except ValueError as err: + return SetLEDBrightnessResponse(False, f'{err}') - return SetLEDBrightnessResponse(True, f'Changed brightness to {req.data}') + return SetLEDBrightnessResponse(True, f'Changed brightness to {req.data}') def _clear_panel(self, panel_num: int) -> None: self._set_panel_frame(panel_num, [[0, 0, 0]] * self._num_led) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index f553d2d1e..ecc41c9fa 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -184,63 +184,57 @@ def _watchdog_cb(self, *args) -> None: self._watchdog() def _aux_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - with self._lock: - res = self._set_bool_srv_handle(req.data, self._pins.AUX_PW_EN, 'Aux power enable') - if res.success: - self._publish_io_state('aux_power', req.data) - return res + res = self._set_bool_srv_handle(req.data, self._pins.AUX_PW_EN, 'Aux power enable') + if res.success: + self._publish_io_state('aux_power', req.data) + return res def _charger_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - with self._lock: - res = self._set_bool_srv_handle(not req.data, self._pins.CHRG_DISABLE, 'Charger disable') - if res.success: - self._publish_io_state('charger_enabled', req.data) - return res + res = self._set_bool_srv_handle(not req.data, self._pins.CHRG_DISABLE, 'Charger disable') + if res.success: + self._publish_io_state('charger_enabled', req.data) + return res def _digital_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - with self._lock: - res = self._set_bool_srv_handle(req.data, self._pins.VDIG_OFF, 'Digital power enable') - if res.success: - self._publish_io_state('digital_power', req.data) - return res + res = self._set_bool_srv_handle(req.data, self._pins.VDIG_OFF, 'Digital power enable') + if res.success: + self._publish_io_state('digital_power', req.data) + return res def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: - if self._validate_gpio_pin(self._pins.E_STOP_RESET, False): - return TriggerResponse(True, 'E-STOP is not active, reset is not needed') - elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: - return TriggerResponse( - False, - 'E-STOP reset failed, messages are still published on /cmd_vel topic!', - ) - elif self._can_net_err: - return TriggerResponse( - False, - 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', - ) - - self._reset_e_stop() - - if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): - self._watchdog.turn_off() - return TriggerResponse( - False, - 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', - ) - - return TriggerResponse(True, 'E-STOP reset successful') + if self._validate_gpio_pin(self._pins.E_STOP_RESET, False): + return TriggerResponse(True, 'E-STOP is not active, reset is not needed') + elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: + return TriggerResponse( + False, + 'E-STOP reset failed, messages are still published on /cmd_vel topic!', + ) + elif self._can_net_err: + return TriggerResponse( + False, + 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', + ) + + self._reset_e_stop() + + if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): + self._watchdog.turn_off() + return TriggerResponse( + False, + 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', + ) + + return TriggerResponse(True, 'E-STOP reset successful') def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: - self._watchdog.turn_off() - return TriggerResponse(True, f'E-STOP triggered, watchdog turned off') + self._watchdog.turn_off() + return TriggerResponse(True, f'E-STOP triggered, watchdog turned off') def _fan_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: - with self._lock: - res = self._set_bool_srv_handle(req.data, self._pins.FAN_SW, 'Fan enable') - if res.success: - self._publish_io_state('fan', req.data) - return res + res = self._set_bool_srv_handle(req.data, self._pins.FAN_SW, 'Fan enable') + if res.success: + self._publish_io_state('fan', req.data) + return res def _set_bool_srv_handle(self, value: bool, pin: int, name: str) -> SetBoolResponse: rospy.logdebug(f'[{rospy.get_name()}] Requested {name} = {value}') From 4959c3061e75b84d9f3ea581b0f97498ccf75e49 Mon Sep 17 00:00:00 2001 From: aayli Date: Fri, 21 Apr 2023 16:52:42 +0200 Subject: [PATCH 5/7] =?UTF-8?q?Pawe=C5=82=20suggestions?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- panther_battery/src/adc_node.py | 124 +++++----- .../src/roboteq_republisher_node.py | 4 +- panther_driver/src/driver_node.py | 228 +++++++++--------- panther_lights/src/controller_node.py | 80 +++--- panther_lights/src/scheduler_node.py | 10 +- panther_manager/src/manager_node.py | 22 +- panther_power_control/src/power_board_node.py | 65 ++--- panther_power_control/src/relays_node.py | 19 +- 8 files changed, 288 insertions(+), 264 deletions(-) diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 0afe12841..b8f0d501e 100755 --- a/panther_battery/src/adc_node.py +++ b/panther_battery/src/adc_node.py @@ -18,7 +18,7 @@ def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) self._lock = Lock() - + self._V_driv_front = None self._V_driv_rear = None self._I_driv_front = None @@ -74,64 +74,63 @@ def _io_state_cb(self, io_state: IOState) -> None: self._charger_connected = io_state.charger_connected def _battery_timer_cb(self, *args) -> None: - with self._lock: - try: - V_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage0_raw', LSB=0.02504255, offset=0.0 - ) - V_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage3_raw', LSB=0.02504255, offset=0.0 - ) - V_temp_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage1_raw', LSB=0.002, offset=0.0 - ) - V_temp_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage0_raw', LSB=0.002, offset=0.0 - ) - I_charge_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage3_raw', LSB=0.005, offset=0.0 - ) - I_charge_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device0/in_voltage2_raw', LSB=0.005, offset=0.0 - ) - I_bat_1 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage2_raw', LSB=0.04, offset=625.0 - ) - I_bat_2 = self._get_adc_measurement( - path='/sys/bus/iio/devices/iio:device1/in_voltage1_raw', LSB=0.04, offset=625.0 - ) - except: - rospy.logerr(f'[{rospy.get_name()}] Battery ADC measurement error') - return + try: + V_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage0_raw', LSB=0.02504255, offset=0.0 + ) + V_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage3_raw', LSB=0.02504255, offset=0.0 + ) + V_temp_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage1_raw', LSB=0.002, offset=0.0 + ) + V_temp_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage0_raw', LSB=0.002, offset=0.0 + ) + I_charge_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage3_raw', LSB=0.005, offset=0.0 + ) + I_charge_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device0/in_voltage2_raw', LSB=0.005, offset=0.0 + ) + I_bat_1 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage2_raw', LSB=0.04, offset=625.0 + ) + I_bat_2 = self._get_adc_measurement( + path='/sys/bus/iio/devices/iio:device1/in_voltage1_raw', LSB=0.04, offset=625.0 + ) + except: + rospy.logerr(f'[{rospy.get_name()}] Battery ADC measurement error') + return - if self._battery_count == 2: - temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) - temp_bat_2 = self._voltage_to_deg(V_temp_bat_2) + if self._battery_count == 2: + temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) + temp_bat_2 = self._voltage_to_deg(V_temp_bat_2) - self._publish_battery_msg( - self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1 - ) - self._publish_battery_msg( - self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2 - ) + self._publish_battery_msg( + self._battery_1_pub, V_bat_1, temp_bat_1, -I_bat_1 + I_charge_bat_1 + ) + self._publish_battery_msg( + self._battery_2_pub, V_bat_2, temp_bat_2, -I_bat_2 + I_charge_bat_2 + ) - V_bat_avereage = (V_bat_1 + V_bat_2) / 2.0 - temp_bat_average = (temp_bat_1 + temp_bat_2) / 2.0 - I_bat_sum = I_bat_1 + I_bat_2 - I_charge_bat = I_charge_bat_1 + I_charge_bat_2 + V_bat_avereage = (V_bat_1 + V_bat_2) / 2.0 + temp_bat_average = (temp_bat_1 + temp_bat_2) / 2.0 + I_bat_sum = I_bat_1 + I_bat_2 + I_charge_bat = I_charge_bat_1 + I_charge_bat_2 - self._publish_battery_msg( - self._battery_pub, - V_bat_avereage, - temp_bat_average, - -I_bat_sum + I_charge_bat, - ) + self._publish_battery_msg( + self._battery_pub, + V_bat_avereage, + temp_bat_average, + -I_bat_sum + I_charge_bat, + ) - else: - temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) - self._publish_battery_msg( - self._battery_pub, V_bat_1, temp_bat_1, -(I_bat_1 + I_bat_2) + I_charge_bat_1 - ) + else: + temp_bat_1 = self._voltage_to_deg(V_temp_bat_1) + self._publish_battery_msg( + self._battery_pub, V_bat_1, temp_bat_1, -(I_bat_1 + I_bat_2) + I_charge_bat_1 + ) def _check_battery_count(self) -> int: trials_num = 10 @@ -183,15 +182,16 @@ def _publish_battery_msg( battery_msg.present = True # check battery status - if self._charger_connected: - if battery_msg.percentage >= 1.0: - battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL - elif self._battery_charging: - battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING + with self._lock: + if self._charger_connected: + if battery_msg.percentage >= 1.0: + battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_FULL + elif self._battery_charging: + battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING + else: + battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING else: - battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING - else: - battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING + battery_msg.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_DISCHARGING bat_pub.publish(battery_msg) diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 23ad3532a..ef1d45f02 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -13,8 +13,8 @@ class RoboteqRepublisherNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) - self._lock = Lock() - + self._lock = Lock() + self._battery_voltage = None self._battery_current = None self._battery_timeout = 1.0 diff --git a/panther_driver/src/driver_node.py b/panther_driver/src/driver_node.py index 072d96a3f..14cbed3b5 100755 --- a/panther_driver/src/driver_node.py +++ b/panther_driver/src/driver_node.py @@ -68,7 +68,7 @@ def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) self._lock = Lock() - + self._eds_file = rospy.get_param('~eds_file') self._use_pdo = rospy.get_param('~use_pdo', False) self._can_interface = rospy.get_param('~can_interface', 'panther_can') @@ -261,135 +261,135 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _main_timer_cb(self, *args) -> None: - with self._lock: - time_now = rospy.Time.now() - dt = (time_now - self._time_last).to_sec() - self._time_last = time_now - - if (time_now - self._cmd_vel_command_last_time) < rospy.Duration( - secs=self._cmd_vel_timeout - ): - self._panther_can.write_wheels_enc_velocity(self._panther_kinematics.wheels_enc_speed) - else: - self._panther_can.write_wheels_enc_velocity([0.0, 0.0, 0.0, 0.0]) - - wheel_enc_pos = self._panther_can.query_wheels_enc_pose() - wheel_enc_vel = self._panther_can.query_wheels_enc_velocity() - wheel_enc_curr = self._panther_can.query_motor_current() - - # convert tics to rad - self._wheels_ang_pos = [ - (2.0 * math.pi) * (pos / (self._encoder_resolution * self._gear_ratio)) - for pos in wheel_enc_pos - ] - # convert RPM to rad/s - self._wheels_ang_vel = [ - (2.0 * math.pi / 60.0) * (vel / self._gear_ratio) for vel in wheel_enc_vel - ] - # convert A to Nm - self._motors_effort = [ - enc_curr * self._motor_torque_constant for enc_curr in wheel_enc_curr - ] - - try: - self._robot_pos, self._robot_vel = self._panther_kinematics.forward_kinematics( - *self._wheels_ang_vel, dt=dt - ) - except: - rospy.logwarn(f'[{rospy.get_name()}] Could not get robot pose') - - self._robot_orientation_quat = quaternion_from_euler(0.0, 0.0, self._robot_pos[2]) - - if self._publish_joints: - self._publish_joint_state_cb() - if self._publish_pose: - self._publish_pose_cb() - if self._publish_odom: - self._publish_odom_cb() - if self._publish_tf: - self._publish_tf_cb() + time_now = rospy.Time.now() + dt = (time_now - self._time_last).to_sec() + self._time_last = time_now + + if (time_now - self._cmd_vel_command_last_time) < rospy.Duration( + secs=self._cmd_vel_timeout + ): + self._panther_can.write_wheels_enc_velocity( + self._panther_kinematics.wheels_enc_speed + ) + else: + self._panther_can.write_wheels_enc_velocity([0.0, 0.0, 0.0, 0.0]) + + wheel_enc_pos = self._panther_can.query_wheels_enc_pose() + wheel_enc_vel = self._panther_can.query_wheels_enc_velocity() + wheel_enc_curr = self._panther_can.query_motor_current() + + # convert tics to rad + self._wheels_ang_pos = [ + (2.0 * math.pi) * (pos / (self._encoder_resolution * self._gear_ratio)) + for pos in wheel_enc_pos + ] + # convert RPM to rad/s + self._wheels_ang_vel = [ + (2.0 * math.pi / 60.0) * (vel / self._gear_ratio) for vel in wheel_enc_vel + ] + # convert A to Nm + self._motors_effort = [ + enc_curr * self._motor_torque_constant for enc_curr in wheel_enc_curr + ] + + try: + self._robot_pos, self._robot_vel = self._panther_kinematics.forward_kinematics( + *self._wheels_ang_vel, dt=dt + ) + except: + rospy.logwarn(f'[{rospy.get_name()}] Could not get robot pose') + + self._robot_orientation_quat = quaternion_from_euler(0.0, 0.0, self._robot_pos[2]) + + if self._publish_joints: + self._publish_joint_state_cb() + if self._publish_pose: + self._publish_pose_cb() + if self._publish_odom: + self._publish_odom_cb() + if self._publish_tf: + self._publish_tf_cb() def _driver_state_timer_cb(self, *args) -> None: - with self._lock: - [ - self._driver_state_msg.front.current, - self._driver_state_msg.front.voltage, - self._driver_state_msg.rear.current, - self._driver_state_msg.rear.voltage, - ] = self._panther_can.query_battery_data() - - [ - self._driver_state_msg.front.temperature, - self._driver_state_msg.rear.temperature, - ] = self._panther_can.query_driver_temperature_data() - - [self._driver_state_msg.front.fault_flag, self._driver_state_msg.rear.fault_flag] = [ - self._driver_flag_loggers['fault_flags'](flag_val) - for flag_val in self._panther_can.query_fault_flags() - ] - - ( - self._driver_state_msg.front.fault_flag.can_net_err, - self._driver_state_msg.rear.fault_flag.can_net_err, - ) = self._panther_can.can_connection_error() - - [self._driver_state_msg.front.script_flag, self._driver_state_msg.rear.script_flag] = [ - self._driver_flag_loggers['script_flags'](flag_val) - for flag_val in self._panther_can.query_script_flags() - ] - - [ - self._driver_state_msg.front.right_motor.runtime_error, - self._driver_state_msg.front.left_motor.runtime_error, - self._driver_state_msg.rear.right_motor.runtime_error, - self._driver_state_msg.rear.left_motor.runtime_error, - ] = [ - self._driver_flag_loggers['runtime_errors'](flag_val) - for flag_val in self._panther_can.query_runtime_stat_flag() - ] - - self._driver_state_pub.publish(self._driver_state_msg) + [ + self._driver_state_msg.front.current, + self._driver_state_msg.front.voltage, + self._driver_state_msg.rear.current, + self._driver_state_msg.rear.voltage, + ] = self._panther_can.query_battery_data() + + [ + self._driver_state_msg.front.temperature, + self._driver_state_msg.rear.temperature, + ] = self._panther_can.query_driver_temperature_data() + + [self._driver_state_msg.front.fault_flag, self._driver_state_msg.rear.fault_flag] = [ + self._driver_flag_loggers['fault_flags'](flag_val) + for flag_val in self._panther_can.query_fault_flags() + ] + + ( + self._driver_state_msg.front.fault_flag.can_net_err, + self._driver_state_msg.rear.fault_flag.can_net_err, + ) = self._panther_can.can_connection_error() + + [self._driver_state_msg.front.script_flag, self._driver_state_msg.rear.script_flag] = [ + self._driver_flag_loggers['script_flags'](flag_val) + for flag_val in self._panther_can.query_script_flags() + ] + + [ + self._driver_state_msg.front.right_motor.runtime_error, + self._driver_state_msg.front.left_motor.runtime_error, + self._driver_state_msg.rear.right_motor.runtime_error, + self._driver_state_msg.rear.left_motor.runtime_error, + ] = [ + self._driver_flag_loggers['runtime_errors'](flag_val) + for flag_val in self._panther_can.query_runtime_stat_flag() + ] + + self._driver_state_pub.publish(self._driver_state_msg) def _safety_timer_cb(self, *args) -> None: with self._lock: - if any(self._panther_can.can_connection_error()): - if not self._e_stop_cliented: - self._trigger_panther_e_stop() - self._stop_cmd_vel_cb = True - - rospy.logerr_throttle( - 10.0, - f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure). ' - f'Please ensure that controllers are powered on.', - ) - elif self._e_stop_cliented: + e_stop_cliented = self._e_stop_cliented + + if any(self._panther_can.can_connection_error()): + if not e_stop_cliented: + self._trigger_panther_e_stop() self._stop_cmd_vel_cb = True - else: - self._stop_cmd_vel_cb = False + + rospy.logerr_throttle( + 10.0, + f'[{rospy.get_name()}] Unable to communicate with motor controllers (CAN interface connection failure). ' + f'Please ensure that controllers are powered on.', + ) + elif e_stop_cliented: + self._stop_cmd_vel_cb = True + else: + self._stop_cmd_vel_cb = False def _e_stop_cb(self, data: Bool) -> None: with self._lock: self._e_stop_cliented = data.data def _cmd_vel_cb(self, data: Twist) -> None: - with self._lock: - # Block all motors if any Roboteq controller returns a fault flag or runtime error flag - if not self._stop_cmd_vel_cb: - self._panther_kinematics.inverse_kinematics(data) - else: - self._panther_kinematics.inverse_kinematics(Twist()) + # Block all motors if any Roboteq controller returns a fault flag or runtime error flag + if not self._stop_cmd_vel_cb: + self._panther_kinematics.inverse_kinematics(data) + else: + self._panther_kinematics.inverse_kinematics(Twist()) - self._cmd_vel_command_last_time = rospy.Time.now() + self._cmd_vel_command_last_time = rospy.Time.now() def _reset_roboteq_script_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: - try: - if self._panther_can.restart_roboteq_script(): - return TriggerResponse(True, 'Roboteq script reset successful.') - except Exception as err: - return TriggerResponse(False, f'Roboteq script reset failed: \n{err}') + try: + if self._panther_can.restart_roboteq_script(): + return TriggerResponse(True, 'Roboteq script reset successful.') + except Exception as err: + return TriggerResponse(False, f'Roboteq script reset failed: \n{err}') - return TriggerResponse(False, f'Roboteq script reset failed') + return TriggerResponse(False, f'Roboteq script reset failed') def _publish_joint_state_cb(self) -> None: self._joint_state_msg.header.stamp = rospy.Time.now() diff --git a/panther_lights/src/controller_node.py b/panther_lights/src/controller_node.py index 78b113e3c..1cc04d448 100755 --- a/panther_lights/src/controller_node.py +++ b/panther_lights/src/controller_node.py @@ -18,6 +18,7 @@ from animation import Animation, BASIC_ANIMATIONS + class PantherAnimation: ANIMATION_DEFAULT_PRIORITY: int = 3 ANIMATION_DEFAULT_TIMEOUT: float = 120.0 @@ -239,53 +240,52 @@ def _animation_queue_timer_cb(self, *args) -> None: self._animation_queue_pub.publish(anim_queue_msg) def _set_animation_cb(self, req: SetLEDAnimationRequest) -> SetLEDAnimationResponse: - with self._lock: - if not req.animation.id in self._animations: - return SetLEDAnimationResponse(False, f'No Animation with id: {req.animation.id}') - - try: - animation = deepcopy(self._animations[req.animation.id]) - animation.front.set_param(req.animation.param) - animation.rear.set_param(req.animation.param) - animation.reset_time() - animation.repeating = req.repeating - self._add_animation_to_queue(animation) - except ValueError as err: - return SetLEDAnimationResponse(False, f'Failed to add animation to queue: {err}') - - return SetLEDAnimationResponse( - True, f'Successfully set an animation with id {req.animation.id}' - ) - - def _set_image_animation_cb(self, req: SetLEDImageAnimationRequest) -> SetLEDImageAnimationResponse: - with self._lock: - animation = PantherAnimation() + if not req.animation.id in self._animations: + return SetLEDAnimationResponse(False, f'No Animation with id: {req.animation.id}') + + try: + animation = deepcopy(self._animations[req.animation.id]) + animation.front.set_param(req.animation.param) + animation.rear.set_param(req.animation.param) + animation.reset_time() animation.repeating = req.repeating + self._add_animation_to_queue(animation) + except ValueError as err: + return SetLEDAnimationResponse(False, f'Failed to add animation to queue: {err}') + + return SetLEDAnimationResponse( + True, f'Successfully set an animation with id {req.animation.id}' + ) - try: - animation_description_front = self._get_image_animation_description(req.front) - animation_description_rear = self._get_image_animation_description(req.rear) + def _set_image_animation_cb( + self, req: SetLEDImageAnimationRequest + ) -> SetLEDImageAnimationResponse: + animation = PantherAnimation() + animation.repeating = req.repeating - animation.front = BASIC_ANIMATIONS['image_animation']( - animation_description_front, self._num_led, self._controller_frequency - ) - animation.rear = BASIC_ANIMATIONS['image_animation']( - animation_description_rear, self._num_led, self._controller_frequency - ) + try: + animation_description_front = self._get_image_animation_description(req.front) + animation_description_rear = self._get_image_animation_description(req.rear) - self._add_animation_to_queue(animation) - except Exception as err: - return SetLEDImageAnimationResponse(False, f'{err}') + animation.front = BASIC_ANIMATIONS['image_animation']( + animation_description_front, self._num_led, self._controller_frequency + ) + animation.rear = BASIC_ANIMATIONS['image_animation']( + animation_description_rear, self._num_led, self._controller_frequency + ) + + self._add_animation_to_queue(animation) + except Exception as err: + return SetLEDImageAnimationResponse(False, f'{err}') - return SetLEDImageAnimationResponse(True, f'Successfully set custom animation') + return SetLEDImageAnimationResponse(True, f'Successfully set custom animation') def _update_animations_cb(self, req: TriggerRequest) -> TriggerResponse: - with self._lock: - try: - self._update_user_animations() - except Exception as err: - return TriggerResponse(False, f'Failed to update animations: {err}') - return TriggerResponse(True, 'Animations updated successfully') + try: + self._update_user_animations() + except Exception as err: + return TriggerResponse(False, f'Failed to update animations: {err}') + return TriggerResponse(True, 'Animations updated successfully') def _rgb_frame_to_img_msg(self, rgb_frame: list, brightness: int, frame_id: str) -> Image: img_msg = Image() diff --git a/panther_lights/src/scheduler_node.py b/panther_lights/src/scheduler_node.py index d3c242f8d..709065dbc 100755 --- a/panther_lights/src/scheduler_node.py +++ b/panther_lights/src/scheduler_node.py @@ -15,7 +15,7 @@ def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) self._lock = Lock() - + self._battery_percentage = 1.0 self._battery_status = None self._charging_percentage = -1.0 # -1.0 to trigger animation when charger gets connected @@ -29,7 +29,9 @@ def __init__(self, name: str) -> None: ] self._critical_battery_anim_period = rospy.get_param('~critical_battery_anim_period', 15.0) - self._critical_battery_threshold_percent = rospy.get_param('~critical_battery_threshold_percent', 0.1) + self._critical_battery_threshold_percent = rospy.get_param( + '~critical_battery_threshold_percent', 0.1 + ) self._battery_state_anim_period = rospy.get_param('~battery_state_anim_period', 120.0) self._low_battery_anim_period = rospy.get_param('~low_battery_anim_period', 30.0) self._low_battery_threshold_percent = rospy.get_param('~low_battery_threshold_percent', 0.4) @@ -39,7 +41,9 @@ def __init__(self, name: str) -> None: # Subscribers # ------------------------------- - self._battery_sub = rospy.Subscriber('battery', BatteryState, self._battery_cb, queue_size=1) + self._battery_sub = rospy.Subscriber( + 'battery', BatteryState, self._battery_cb, queue_size=1 + ) self._e_stop_sub = rospy.Subscriber('hardware/e_stop', Bool, self._e_stop_cb, queue_size=1) # ------------------------------- diff --git a/panther_manager/src/manager_node.py b/panther_manager/src/manager_node.py index bac0a9429..d841c6813 100755 --- a/panther_manager/src/manager_node.py +++ b/panther_manager/src/manager_node.py @@ -20,6 +20,7 @@ def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) self._lock = Lock() + self._aux_power_state = None self._e_stop_state = None self._fan_state = None @@ -108,16 +109,22 @@ def __init__(self, name: str) -> None: # ------------------------------- self._battery_sub = rospy.Subscriber('battery', BatteryState, self._battery_cb) - self._driver_state_sub = rospy.Subscriber('driver/motor_controllers_state', DriverState, self._driver_state_cb) + self._driver_state_sub = rospy.Subscriber( + 'driver/motor_controllers_state', DriverState, self._driver_state_cb + ) self._e_stop_sub = rospy.Subscriber('hardware/e_stop', Bool, self._e_stop_cb) self._io_state_sub = rospy.Subscriber('hardware/io_state', IOState, self._io_state_cb) - self._system_status_sub = rospy.Subscriber('system_status', SystemStatus, self._system_status_cb) + self._system_status_sub = rospy.Subscriber( + 'system_status', SystemStatus, self._system_status_cb + ) # ------------------------------- # Service servers # ------------------------------- - self._overwrite_fan_control_server = rospy.Service('manager/overwrite_fan_control', SetBool, self._overwrite_fan_control_cb) + self._overwrite_fan_control_server = rospy.Service( + 'manager/overwrite_fan_control', SetBool, self._overwrite_fan_control_cb + ) # ------------------------------- # Service clients @@ -150,7 +157,10 @@ def _battery_cb(self, battery_state: BatteryState) -> None: def _driver_state_cb(self, driver_state: DriverState) -> None: with self._lock: - if self._front_driver_temp_window is not None and self._rear_driver_temp_window is not None: + if ( + self._front_driver_temp_window is not None + and self._rear_driver_temp_window is not None + ): self._front_driver_temp_window = self._move_window( self._front_driver_temp_window, driver_state.front.temperature ) @@ -178,7 +188,9 @@ def _io_state_cb(self, io_state: IOState) -> None: def _system_status_cb(self, system_status: SystemStatus) -> None: with self._lock: if self._cpu_temp_window is not None: - self._cpu_temp_window = self._move_window(self._cpu_temp_window, system_status.cpu_temp) + self._cpu_temp_window = self._move_window( + self._cpu_temp_window, system_status.cpu_temp + ) else: self._cpu_temp_window = [system_status.cpu_temp] * self._cpu_window_len diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index ecc41c9fa..d69cf3edb 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -59,7 +59,9 @@ class PowerBoardNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) - self._lock = Lock() + self._pins_lock = Lock() + self._e_stop_lock = Lock() + self._clearing_e_stop = False self._pins = PatherGPIO() @@ -148,15 +150,17 @@ def __init__(self, name: str) -> None: rospy.loginfo(f'[{rospy.get_name()}] Node started') def _cmd_vel_cb(self, *args) -> None: - with self._lock: + with self._e_stop_lock: self._cmd_vel_msg_time = rospy.get_time() def _motor_controllers_state_cb(self, msg: DriverState) -> None: - with self._lock: - self._can_net_err = any({msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err}) + with self._e_stop_lock: + self._can_net_err = any( + {msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err} + ) def _gpio_interrupt_cb(self, pin: int) -> None: - with self._lock: + with self._pins_lock: if pin == self._pins.SHDN_INIT: self._chrg_sense_interrupt_time = rospy.get_time() @@ -164,7 +168,7 @@ def _gpio_interrupt_cb(self, pin: int) -> None: self._e_stop_interrupt_time = rospy.get_time() def _publish_pin_state_cb(self, *args) -> None: - with self._lock: + with self._pins_lock: charger_state = self._read_pin(self._pins.CHRG_SENSE) self._publish_io_state('charger_connected', charger_state) @@ -180,7 +184,7 @@ def _publish_pin_state_cb(self, *args) -> None: self._e_stop_interrupt_time = float('inf') def _watchdog_cb(self, *args) -> None: - with self._lock: + with self._pins_lock: self._watchdog() def _aux_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: @@ -202,29 +206,30 @@ def _digital_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: return res def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: - if self._validate_gpio_pin(self._pins.E_STOP_RESET, False): - return TriggerResponse(True, 'E-STOP is not active, reset is not needed') - elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: - return TriggerResponse( - False, - 'E-STOP reset failed, messages are still published on /cmd_vel topic!', - ) - elif self._can_net_err: - return TriggerResponse( - False, - 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', - ) - - self._reset_e_stop() - - if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): - self._watchdog.turn_off() - return TriggerResponse( - False, - 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', - ) - - return TriggerResponse(True, 'E-STOP reset successful') + with self._e_stop_lock: + if self._validate_gpio_pin(self._pins.E_STOP_RESET, False): + return TriggerResponse(True, 'E-STOP is not active, reset is not needed') + elif rospy.get_time() - self._cmd_vel_msg_time <= 2.0: + return TriggerResponse( + False, + 'E-STOP reset failed, messages are still published on /cmd_vel topic!', + ) + elif self._can_net_err: + return TriggerResponse( + False, + 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', + ) + + self._reset_e_stop() + + if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): + self._watchdog.turn_off() + return TriggerResponse( + False, + 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', + ) + + return TriggerResponse(True, 'E-STOP reset successful') def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: self._watchdog.turn_off() diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 5238df65e..f2579624a 100755 --- a/panther_power_control/src/relays_node.py +++ b/panther_power_control/src/relays_node.py @@ -67,7 +67,9 @@ def __init__(self, name: str) -> None: # ------------------------------- # check motor state at 10 Hz - self._set_motor_state_timer = rospy.Timer(rospy.Duration(0.1), self._set_motor_state_timer_cb) + self._set_motor_state_timer = rospy.Timer( + rospy.Duration(0.1), self._set_motor_state_timer_cb + ) # init e-stop state self._e_stop_state_pub.publish(self._e_stop_state) @@ -81,7 +83,9 @@ def _cmd_vel_cb(self, *args) -> None: def _motor_controllers_state_cb(self, msg: DriverState) -> None: with self._lock: - self._can_net_err = any({msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err}) + self._can_net_err = any( + {msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err} + ) def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: with self._lock: @@ -95,7 +99,7 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: False, 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', ) - + self._e_stop_state = False self._e_stop_state_pub.publish(self._e_stop_state) return TriggerResponse(True, 'E-STOP reset successful') @@ -107,11 +111,10 @@ def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: return TriggerResponse(True, 'E-SROP triggered successful') def _set_motor_state_timer_cb(self, *args) -> None: - with self._lock: - motor_state = GPIO.input(self._pins.STAGE2_INPUT) - GPIO.output(self._pins.MOTOR_ON, motor_state) - if motor_state != self._motor_on_pub.impl.latch.data: - self._motor_on_pub.publish(motor_state) + motor_state = GPIO.input(self._pins.STAGE2_INPUT) + GPIO.output(self._pins.MOTOR_ON, motor_state) + if motor_state != self._motor_on_pub.impl.latch.data: + self._motor_on_pub.publish(motor_state) def _setup_gpio(self) -> None: GPIO.setwarnings(False) From a0daef5e3b0094fff3c1557246b924ff0866a4d5 Mon Sep 17 00:00:00 2001 From: aayli Date: Fri, 21 Apr 2023 17:03:26 +0200 Subject: [PATCH 6/7] change --- panther_power_control/src/power_board_node.py | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index d69cf3edb..20138e55b 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -220,16 +220,16 @@ def _e_stop_reset_cb(self, req: TriggerRequest) -> TriggerResponse: 'E-STOP reset failed, unable to communicate with motor controllers! Please check connection with motor controllers.', ) - self._reset_e_stop() + self._reset_e_stop() - if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): - self._watchdog.turn_off() - return TriggerResponse( - False, - 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', - ) + if self._validate_gpio_pin(self._pins.E_STOP_RESET, True): + self._watchdog.turn_off() + return TriggerResponse( + False, + 'E-STOP reset failed, check for pressed E-STOP buttons or other triggers', + ) - return TriggerResponse(True, 'E-STOP reset successful') + return TriggerResponse(True, 'E-STOP reset successful') def _e_stop_trigger_cb(self, req: TriggerRequest) -> TriggerResponse: self._watchdog.turn_off() @@ -252,17 +252,18 @@ def _set_bool_srv_handle(self, value: bool, pin: int, name: str) -> SetBoolRespo return SetBoolResponse(success, msg) def _reset_e_stop(self) -> None: - self._clearing_e_stop = True - GPIO.setup(self._pins.E_STOP_RESET, GPIO.OUT) - self._watchdog.turn_on() + with self._pins_lock: + self._clearing_e_stop = True + GPIO.setup(self._pins.E_STOP_RESET, GPIO.OUT) + self._watchdog.turn_on() - self._write_to_pin(self._pins.E_STOP_RESET, False) - rospy.sleep(0.1) + self._write_to_pin(self._pins.E_STOP_RESET, False) + rospy.sleep(0.1) - GPIO.setup(self._pins.E_STOP_RESET, GPIO.IN) - rospy.sleep(0.1) - self._clearing_e_stop = False - self._e_stop_event() + GPIO.setup(self._pins.E_STOP_RESET, GPIO.IN) + rospy.sleep(0.1) + self._clearing_e_stop = False + self._e_stop_event() def _e_stop_event(self) -> None: e_stop_state = self._read_pin(self._pins.E_STOP_RESET) From e8c1602f83db41cb4f2ecd5df5f6107968cf35ae Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Mon, 24 Apr 2023 10:23:21 +0200 Subject: [PATCH 7/7] e_stop --- panther_power_control/src/power_board_node.py | 22 +++++++++---------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/panther_power_control/src/power_board_node.py b/panther_power_control/src/power_board_node.py index 20138e55b..05318af50 100755 --- a/panther_power_control/src/power_board_node.py +++ b/panther_power_control/src/power_board_node.py @@ -184,8 +184,7 @@ def _publish_pin_state_cb(self, *args) -> None: self._e_stop_interrupt_time = float('inf') def _watchdog_cb(self, *args) -> None: - with self._pins_lock: - self._watchdog() + self._watchdog() def _aux_power_enable_cb(self, req: SetBoolRequest) -> SetBoolResponse: res = self._set_bool_srv_handle(req.data, self._pins.AUX_PW_EN, 'Aux power enable') @@ -252,18 +251,17 @@ def _set_bool_srv_handle(self, value: bool, pin: int, name: str) -> SetBoolRespo return SetBoolResponse(success, msg) def _reset_e_stop(self) -> None: - with self._pins_lock: - self._clearing_e_stop = True - GPIO.setup(self._pins.E_STOP_RESET, GPIO.OUT) - self._watchdog.turn_on() + self._clearing_e_stop = True + GPIO.setup(self._pins.E_STOP_RESET, GPIO.OUT) + self._watchdog.turn_on() - self._write_to_pin(self._pins.E_STOP_RESET, False) - rospy.sleep(0.1) + self._write_to_pin(self._pins.E_STOP_RESET, False) + rospy.sleep(0.1) - GPIO.setup(self._pins.E_STOP_RESET, GPIO.IN) - rospy.sleep(0.1) - self._clearing_e_stop = False - self._e_stop_event() + GPIO.setup(self._pins.E_STOP_RESET, GPIO.IN) + rospy.sleep(0.1) + self._clearing_e_stop = False + self._e_stop_event() def _e_stop_event(self) -> None: e_stop_state = self._read_pin(self._pins.E_STOP_RESET)