Revamp Callback Groups (#92)

* Put MoveIt2 objects into their own ReentrantCallbackGroup

* Revert create_action_servers.py default callback group to MutuallyExclusive

* Updated watchdog callback groups

* Updated face detection callback groups

* Updated food detection callback groups

* Updated republisher callback groups

ada_feeding/ada_feeding/behaviors/move_to.py

@@ -15,6 +15,7 @@
 import py_trees
 from pymoveit2 import MoveIt2, MoveIt2State
 from pymoveit2.robots import kinova
+from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 from rclpy.qos import QoSPresetProfiles
 from sensor_msgs.msg import JointState
@@ -120,12 +121,16 @@ def __init__(
         # Create MoveIt 2 interface for moving the Jaco arm. This must be done
         # in __init__ and not setup since the MoveIt2 interface must be
         # initialized before the ROS2 node starts spinning.
+        # Using ReentrantCallbackGroup to align with the examples from pymoveit2.
+        # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2.
+        callback_group = ReentrantCallbackGroup()
         self.moveit2 = MoveIt2(
             node=self.node,
             joint_names=kinova.joint_names(),
             base_link_name=kinova.base_link_name(),
             end_effector_name="forkTip",
             group_name=kinova.MOVE_GROUP_ARM,
+            callback_group=callback_group,
         )
 
         # Subscribe to the joint state and track the distance to goal while the

ada_feeding/ada_feeding/behaviors/toggle_collision_object.py

@@ -12,6 +12,7 @@
 import py_trees
 from pymoveit2 import MoveIt2
 from pymoveit2.robots import kinova
+from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
 
 # Local imports
@@ -55,12 +56,16 @@ def __init__(
         # Create MoveIt 2 interface for moving the Jaco arm. This must be done
         # in __init__ and not setup since the MoveIt2 interface must be
         # initialized before the ROS2 node starts spinning.
+        # Using ReentrantCallbackGroup to align with the examples from pymoveit2.
+        # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2.
+        callback_group = ReentrantCallbackGroup()
         self.moveit2 = MoveIt2(
             node=self.node,
             joint_names=kinova.joint_names(),
             base_link_name=kinova.base_link_name(),
             end_effector_name="forkTip",
             group_name=kinova.MOVE_GROUP_ARM,
+            callback_group=callback_group,
         )
 
     def update(self) -> py_trees.common.Status:

ada_feeding/ada_feeding/watchdog/ft_sensor_condition.py

@@ -67,11 +67,15 @@ def __init__(self, node: Node) -> None:
 
         # Subscribe to the FT sensor topic
         self.ft_sensor_topic = self._node.resolve_topic_name("~/ft_topic")
+        self.ft_sensor_callback_group = (
+            rclpy.callback_groups.MutuallyExclusiveCallbackGroup()
+        )
         self.ft_sensor_subscription = self._node.create_subscription(
             WrenchStamped,
             self.ft_sensor_topic,
             self.__ft_sensor_callback,
             rclpy.qos.QoSPresetProfiles.SENSOR_DATA.value,
+            callback_group=self.ft_sensor_callback_group,
         )
 
     def __ft_sensor_callback(self, msg: WrenchStamped) -> None:

ada_feeding/scripts/ada_planning_scene.py

@@ -55,6 +55,8 @@ def __init__(self) -> None:
         self.load_parameters()
 
         # Initialize the MoveIt2 interface
+        # Using ReentrantCallbackGroup to align with the examples from pymoveit2.
+        # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2.
         callback_group = ReentrantCallbackGroup()
         self.moveit2 = MoveIt2(
             node=self,

ada_feeding/scripts/ada_watchdog.py

@@ -66,8 +66,16 @@ def __init__(self) -> None:
         )
 
         # Publish at the specified rate
+        # TODO: Consider making this a separate thread that runs at a fixed rate,
+        # to avoid the callback queue getting backed up if the function takes
+        # longer than the specified rate to run.
+        self.timer_callback_group = (
+            rclpy.callback_groups.MutuallyExclusiveCallbackGroup()
+        )
         timer_period = 1.0 / self.publish_rate_hz.value  # seconds
-        self.timer = self.create_timer(timer_period, self.__check_conditions)
+        self.timer = self.create_timer(
+            timer_period, self.__check_conditions, self.timer_callback_group
+        )
 
     def __load_parameters(self) -> None:
         """
@@ -199,7 +207,7 @@ def main(args=None):
     rclpy.init(args=args)
 
     # Use a MultiThreadedExecutor to enable processing topics concurrently
-    executor = MultiThreadedExecutor()
+    executor = MultiThreadedExecutor(num_threads=3)
 
     # Create and spin the node
     ada_watchdog = ADAWatchdog()

ada_feeding/scripts/create_action_servers.py

@@ -89,8 +89,6 @@ def __init__(self) -> None:
         """
         super().__init__("create_action_servers")
 
-        self._default_callback_group = rclpy.callback_groups.ReentrantCallbackGroup()
-
         # Read the parameters that specify what action servers to create.
         action_server_params = self.read_params()
 

ada_feeding_perception/ada_feeding_perception/face_detection.py

@@ -17,6 +17,7 @@
 from geometry_msgs.msg import Point
 import numpy as np
 import rclpy
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.node import Node
 from rclpy.parameter import Parameter
 from rclpy.executors import MultiThreadedExecutor
@@ -55,8 +56,6 @@ def __init__(
         """
         super().__init__("face_detection")
 
-        self._default_callback_group = rclpy.callback_groups.ReentrantCallbackGroup()
-
         # Read the parameters
         # NOTE: These parameters are only read once. Any changes after the node
         # is initialized will not be reflected.
@@ -108,6 +107,7 @@ def __init__(
             SetBool,
             "~/toggle_face_detection",
             self.toggle_face_detection_callback,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
         # Create the publishers
@@ -131,7 +131,11 @@ def __init__(
         self.latest_img_msg_lock = threading.Lock()
         image_topic = "~/image"
         self.img_subscription = self.create_subscription(
-            get_img_msg_type(image_topic, self), image_topic, self.camera_callback, 1
+            get_img_msg_type(image_topic, self),
+            image_topic,
+            self.camera_callback,
+            1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
         depth_buffer_size = depth_buffer_size.value
@@ -144,6 +148,7 @@ def __init__(
             aligned_depth_topic,
             self.depth_callback,
             1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
         # Subscribe to the camera info
@@ -152,7 +157,11 @@ def __init__(
         # (updated with subscription)
         self.camera_matrix = [614, 0, 312, 0, 614, 223, 0, 0, 1]
         self.camera_info_subscription = self.create_subscription(
-            CameraInfo, "~/camera_info", self.camera_info_callback, 1
+            CameraInfo,
+            "~/camera_info",
+            self.camera_info_callback,
+            1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
 
     def read_params(
@@ -504,7 +513,9 @@ def run(self) -> None:
             with self.latest_img_msg_lock:
                 rgb_msg = self.latest_img_msg
             if rgb_msg is None:
-                self.get_logger().warn("No RGB image message received.", throttle_duration_sec=1)
+                self.get_logger().warn(
+                    "No RGB image message received.", throttle_duration_sec=1
+                )
                 continue
 
             # Detect the largest face in the RGB image
@@ -522,8 +533,10 @@ def run(self) -> None:
                 if is_face_detected_depth:
                     # Get the 3d location of the mouth
                     face_detection_msg.detected_mouth_center.header = rgb_msg.header
-                    face_detection_msg.detected_mouth_center.point = self.get_stomion_point(
-                        img_mouth_center[0], img_mouth_center[1], depth_mm
+                    face_detection_msg.detected_mouth_center.point = (
+                        self.get_stomion_point(
+                            img_mouth_center[0], img_mouth_center[1], depth_mm
+                        )
                     )
                 else:
                     is_face_detected = False
@@ -540,7 +553,7 @@ def main(args=None):
     rclpy.init(args=args)
 
     face_detection = FaceDetectionNode()
-    executor = MultiThreadedExecutor()
+    executor = MultiThreadedExecutor(num_threads=4)
 
     # Spin in the background since detecting faces will block
     # the main thread

ada_feeding_perception/ada_feeding_perception/republisher.py

@@ -15,6 +15,7 @@
 # Third-party imports
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 import rclpy
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 
@@ -79,6 +80,7 @@ def __init__(self) -> None:
                 topic=self.from_topics[i],
                 callback=callback,
                 qos_profile=1,  # TODO: we should get and mirror the QOS profile of the from_topic
+                callback_group=MutuallyExclusiveCallbackGroup(),
             )
             self.subs.append(subscriber)
 
@@ -183,7 +185,7 @@ def main(args=None):
     republisher = Republisher()
 
     # Use a MultiThreadedExecutor to enable processing goals concurrently
-    executor = MultiThreadedExecutor()
+    executor = MultiThreadedExecutor(num_threads=len(republisher.subs))
 
     rclpy.spin(republisher, executor=executor)
 

ada_feeding_perception/ada_feeding_perception/segment_from_point.py

@@ -18,6 +18,7 @@
 import rclpy
 from rclpy.action import ActionServer, CancelResponse, GoalResponse
 from rclpy.action.server import ServerGoalHandle
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
 from rclpy.parameter import Parameter
@@ -58,8 +59,6 @@ def __init__(self) -> None:
 
         super().__init__("segment_from_point")
 
-        self._default_callback_group = rclpy.callback_groups.ReentrantCallbackGroup()
-
         # Check if cuda is available
         self.device = "cuda" if torch.cuda.is_available() else "cpu"
 
@@ -91,6 +90,7 @@ def __init__(self) -> None:
             "~/camera_info",
             self.camera_info_callback,
             1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
         self.camera_info = None
         self.camera_info_lock = threading.Lock()
@@ -106,6 +106,7 @@ def __init__(self) -> None:
             aligned_depth_topic,
             self.depth_image_callback,
             1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
         self.latest_depth_img_msg = None
         self.latest_depth_img_msg_lock = threading.Lock()
@@ -117,6 +118,7 @@ def __init__(self) -> None:
             image_topic,
             self.image_callback,
             1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
         self.latest_img_msg = None
         self.latest_img_msg_lock = threading.Lock()
@@ -244,6 +246,7 @@ def initialize_food_segmentation(self, model_name: str, model_path: str) -> None
             self.execute_callback,
             goal_callback=self.goal_callback,
             cancel_callback=self.cancel_callback,
+            callback_group=MutuallyExclusiveCallbackGroup(),
         )
         self.get_logger().info("...Done!")
 
@@ -509,7 +512,7 @@ def main(args=None):
     segment_from_point = SegmentFromPointNode()
 
     # Use a MultiThreadedExecutor to enable processing goals concurrently
-    executor = MultiThreadedExecutor()
+    executor = MultiThreadedExecutor(num_threads=5)
 
     rclpy.spin(segment_from_point, executor=executor)