diff --git a/panther_battery/src/adc_node.py b/panther_battery/src/adc_node.py index 9d5263aa..5092c0a4 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 + from collections import defaultdict import math from threading import Lock @@ -82,7 +84,8 @@ def _motor_controllers_state_cb(self, driver_state: DriverState) -> None: self._I_driv = driver_state.front.current + 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: @@ -195,15 +198,16 @@ def _publish_battery_msg( V_bat_mean = self._count_volt_mean(bat_pub, V_bat) # 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 # check battery health with self._lock: diff --git a/panther_battery/src/roboteq_republisher_node.py b/panther_battery/src/roboteq_republisher_node.py index 5818f882..074e8173 100755 --- a/panther_battery/src/roboteq_republisher_node.py +++ b/panther_battery/src/roboteq_republisher_node.py @@ -17,6 +17,8 @@ class RoboteqRepublisherNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._lock = Lock() + self._battery_voltage: Optional[float] = None self._battery_current: Optional[float] = None self._battery_voltage_mean = 37.0 @@ -26,7 +28,6 @@ def __init__(self, name: str) -> 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 1afee362..2932a694 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 @@ -65,6 +66,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') @@ -269,7 +272,9 @@ def _main_timer_cb(self, *args) -> None: 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) + 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]) @@ -350,8 +355,11 @@ def _driver_state_timer_cb(self, *args) -> None: self._driver_state_pub.publish(self._driver_state_msg) def _safety_timer_cb(self, *args) -> None: + with self._lock: + e_stop_cliented = self._e_stop_cliented + if any(self._panther_can.can_connection_error()): - if not self._e_stop_cliented: + if not e_stop_cliented: self._trigger_panther_e_stop() self._stop_cmd_vel_cb = True @@ -374,7 +382,8 @@ def _safety_timer_cb(self, *args) -> None: 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 diff --git a/panther_lights/src/controller_node.py b/panther_lights/src/controller_node.py index badaeda3..1cc04d44 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 @@ -105,6 +106,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 +177,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: diff --git a/panther_lights/src/scheduler_node.py b/panther_lights/src/scheduler_node.py index 1974f14f..709065db 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 @@ -35,7 +38,7 @@ def __init__(self, name: str) -> None: self._update_charging_anim_step = rospy.get_param('~update_charging_anim_step', 0.1) # ------------------------------- - # Publishers + # Subscribers # ------------------------------- self._battery_sub = rospy.Subscriber( @@ -78,40 +81,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 +138,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 4e777de5..d841c681 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,8 @@ 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 @@ -142,85 +146,97 @@ 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 - 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: - 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 +294,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 2b6c606b..53651c19 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,9 @@ class PowerBoardNode: def __init__(self, name: str) -> None: rospy.init_node(name, anonymous=False) + self._pins_lock = Lock() + self._e_stop_lock = Lock() + self._clearing_e_stop = False self._pins = PatherGPIO() @@ -158,32 +162,38 @@ 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._e_stop_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._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: - if pin == self._pins.SHDN_INIT: - self._chrg_sense_interrupt_time = rospy.get_time() + with self._pins_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._pins_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() @@ -207,18 +217,19 @@ 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.', - ) + 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() diff --git a/panther_power_control/src/relays_node.py b/panther_power_control/src/relays_node.py index 53509cde..650a1450 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() @@ -84,31 +87,37 @@ 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}) - - 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.', + with self._lock: + self._can_net_err = any( + {msg.rear.fault_flag.can_net_err, msg.front.fault_flag.can_net_err} ) - 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_reset_cb(self, req: TriggerRequest) -> TriggerResponse: + 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-STOP 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: motor_state = GPIO.input(self._pins.STAGE2_INPUT)