Skip to content

Commit c808968

Browse files
hello-binithello-binit
committed
Stretch ROS2 driver now uses two callback groups
The first, main group, is a ReentrantCallbackGroup, and covers nearly all callbacks. This means that the multithreaded executor can execute subcription, service, and action callbacks as it likes. The second, mutex group, is a MutuallyExclusiveCallbackGroup, and covers just the main timer in stretch_driver. This main loop handles all publishers and motion execution. The main loop is in a separate group so it doesn't get scheduled in overlapping fashion with itself (which reentrant groups do), while still executing in parallel with the callbacks in main group.
1 parent 62d5d9a commit c808968

File tree

2 files changed

+32
-20
lines changed

2 files changed

+32
-20
lines changed

‎stretch_core/stretch_core/joint_trajectory_server.py

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,6 @@
1515

1616
import rclpy
1717
from rclpy.action import ActionServer, CancelResponse, GoalResponse
18-
from rclpy.callback_groups import ReentrantCallbackGroup
1918
from rclpy.duration import Duration
2019

2120
from control_msgs.action import FollowJointTrajectory
@@ -39,7 +38,7 @@ def __init__(self, node, action_server_rate_hz):
3938
cancel_callback=self.cancel_cb,
4039
goal_callback=self.goal_cb,
4140
handle_accepted_callback=self.handle_accepted_cb,
42-
callback_group=ReentrantCallbackGroup())
41+
callback_group=node.main_group)
4342

4443
self.debug_dir = Path(hu.get_stretch_directory('goals'))
4544
if not self.debug_dir.exists():

‎stretch_core/stretch_core/stretch_driver.py

Lines changed: 31 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
import rclpy
1717
from rclpy.duration import Duration
1818
from rclpy.executors import MultiThreadedExecutor
19-
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
19+
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
2020
from rclpy.node import Node
2121
from rclpy.parameter import Parameter
2222

@@ -970,12 +970,13 @@ def ros_setup(self):
970970
self.is_gamepad_dongle_pub = self.create_publisher(Bool,'is_gamepad_dongle', 1)
971971
self.gamepad_state_pub = self.create_publisher(Joy,'stretch_gamepad_state', 1) # decode using gamepad_conversion.unpack_joy_to_gamepad_state() on client side
972972

973-
self.group = MutuallyExclusiveCallbackGroup()
974-
self.create_subscription(Twist, "cmd_vel", self.set_mobile_base_velocity_callback, 1, callback_group=self.group)
973+
self.main_group = ReentrantCallbackGroup()
974+
self.mutex_group = MutuallyExclusiveCallbackGroup()
975+
self.create_subscription(Twist, "cmd_vel", self.set_mobile_base_velocity_callback, 1, callback_group=self.main_group)
975976

976-
self.create_subscription(Joy, "gamepad_joy", self.set_gamepad_motion_callback, 1, callback_group=self.group)
977+
self.create_subscription(Joy, "gamepad_joy", self.set_gamepad_motion_callback, 1, callback_group=self.main_group)
977978

978-
self.create_subscription(Float64MultiArray, "joint_pose_cmd", self.set_robot_streaming_position_callback, 1, callback_group=self.group)
979+
self.create_subscription(Float64MultiArray, "joint_pose_cmd", self.set_robot_streaming_position_callback, 1, callback_group=self.main_group)
979980

980981
self.declare_parameter('rate', 30.0)
981982
self.joint_state_rate = self.get_parameter('rate').value
@@ -1019,56 +1020,68 @@ def ros_setup(self):
10191020

10201021
self.switch_to_navigation_mode_service = self.create_service(Trigger,
10211022
'/switch_to_navigation_mode',
1022-
self.navigation_mode_service_callback)
1023+
self.navigation_mode_service_callback,
1024+
callback_group=self.main_group)
10231025

10241026
self.switch_to_position_mode_service = self.create_service(Trigger,
10251027
'/switch_to_position_mode',
1026-
self.position_mode_service_callback)
1028+
self.position_mode_service_callback,
1029+
callback_group=self.main_group)
10271030

10281031
self.switch_to_trajectory_mode_service = self.create_service(Trigger,
10291032
'/switch_to_trajectory_mode',
1030-
self.trajectory_mode_service_callback)
1033+
self.trajectory_mode_service_callback,
1034+
callback_group=self.main_group)
10311035

10321036
self.switch_to_gamepad_mode_service = self.create_service(Trigger,
10331037
'/switch_to_gamepad_mode',
1034-
self.gamepad_mode_service_callback)
1038+
self.gamepad_mode_service_callback,
1039+
callback_group=self.main_group)
10351040

10361041
self.activate_streaming_position_service = self.create_service(Trigger,
10371042
'/activate_streaming_position',
1038-
self.activate_streaming_position_service_callback)
1043+
self.activate_streaming_position_service_callback,
1044+
callback_group=self.main_group)
10391045

10401046
self.deactivate_streaming_position_service = self.create_service(Trigger,
10411047
'/deactivate_streaming_position',
1042-
self.deactivate_streaming_position_service_callback)
1048+
self.deactivate_streaming_position_service_callback,
1049+
callback_group=self.main_group)
10431050

10441051
self.stop_the_robot_service = self.create_service(Trigger,
10451052
'/stop_the_robot',
1046-
self.stop_the_robot_callback)
1053+
self.stop_the_robot_callback,
1054+
callback_group=self.main_group)
10471055

10481056
self.home_the_robot_service = self.create_service(Trigger,
10491057
'/home_the_robot',
1050-
self.home_the_robot_callback)
1058+
self.home_the_robot_callback,
1059+
callback_group=self.main_group)
10511060

10521061
self.stow_the_robot_service = self.create_service(Trigger,
10531062
'/stow_the_robot',
1054-
self.stow_the_robot_callback)
1063+
self.stow_the_robot_callback,
1064+
callback_group=self.main_group)
10551065

10561066
self.runstop_service = self.create_service(SetBool,
10571067
'/runstop',
1058-
self.runstop_service_callback)
1068+
self.runstop_service_callback,
1069+
callback_group=self.main_group)
10591070

10601071
self.get_joint_states = self.create_service(Trigger,
10611072
'/get_joint_states',
1062-
self.get_joint_states_callback)
1073+
self.get_joint_states_callback,
1074+
callback_group=self.main_group)
10631075

10641076
self.self_collision_avoidance = self.create_service(SetBool,
10651077
'/self_collision_avoidance',
1066-
self.self_collision_avoidance_callback)
1078+
self.self_collision_avoidance_callback,
1079+
callback_group=self.main_group)
10671080

10681081
# start loop to command the mobile base velocity, publish
10691082
# odometry, and publish joint states
10701083
timer_period = 1.0 / self.joint_state_rate
1071-
self.timer = self.create_timer(timer_period, self.command_mobile_base_velocity_and_publish_state)
1084+
self.timer = self.create_timer(timer_period, self.command_mobile_base_velocity_and_publish_state, callback_group=self.mutex_group)
10721085

10731086

10741087
def main():

0 commit comments

Comments
 (0)