Thread lock

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:

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

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

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: