From de1c8c2cbd27f50982ed01aaddd29b4b53f0eb21 Mon Sep 17 00:00:00 2001 From: yy389 Date: Tue, 10 Oct 2023 01:56:13 -0400 Subject: [PATCH 1/4] multirobot test --- hello_helpers/src/hello_helpers/hello_misc.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 5f8189d..1f48517 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -67,12 +67,13 @@ def get_left_finger_state(joint_states): return [left_finger_position, left_finger_velocity, left_finger_effort] class HelloNode: - def __init__(self): + def __init__(self, robot_name=None): self.joint_states = None self.point_cloud = None self.tool = None self.mode = None self.dryrun = False + self.robot_name = robot_name @classmethod def quick_create(cls, name, wait_for_first_pointcloud=False): @@ -231,7 +232,10 @@ class HelloNode: self.node_name = rospy.get_name() rospy.loginfo("{0} started".format(self.node_name)) - self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction) + if self.robot_name == None: + self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction) + else: + self.trajectory_client = actionlib.SimpleActionClient('/'+self.robot_name+'/follow_joint_trajectory', FollowJointTrajectoryAction) server_reached = self.trajectory_client.wait_for_server(timeout=rospy.Duration(60.0)) if not server_reached: rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.') From 66f36679637c8dcd29a21dda078ab8e802e6d30f Mon Sep 17 00:00:00 2001 From: yy389 Date: Tue, 10 Oct 2023 02:02:39 -0400 Subject: [PATCH 2/4] multirobot revision 1 --- hello_helpers/src/hello_helpers/hello_misc.py | 30 +++++++++++++------ stretch_core/launch/stretch_driver.launch | 4 ++- stretch_core/nodes/joint_trajectory_server.py | 6 +++- stretch_core/nodes/stretch_driver | 18 +++++++++-- 4 files changed, 44 insertions(+), 14 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 1f48517..5d0d944 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -241,24 +241,36 @@ class HelloNode: rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.') sys.exit() - self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback) - + if self.robot_name == None: + self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback) + else: + self._joint_states_subscriber = rospy.Subscriber('/'+self.robot_name+'/joint_states', JointState, self._joint_states_callback) + self.tf2_buffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) + if self.robot_name == None: + self._tool_subscriber = rospy.Subscriber('/tool', String, self._tool_callback) + self._mode_subscriber = rospy.Subscriber('/mode', String, self._mode_callback) + self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self._point_cloud_callback) + else: + self._tool_subscriber = rospy.Subscriber('/'+self.robot_name+'/tool', String, self._tool_callback) + self._mode_subscriber = rospy.Subscriber('/'+self.robot_name+'/mode', String, self._mode_callback) + self.point_cloud_subscriber = rospy.Subscriber('/'+self.robot_name+'/camera/depth/color/points', PointCloud2, self._point_cloud_callback) - self._tool_subscriber = rospy.Subscriber('/tool', String, self._tool_callback) - self._mode_subscriber = rospy.Subscriber('/mode', String, self._mode_callback) - - self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self._point_cloud_callback) self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1) rospy.wait_for_service('/home_the_robot') rospy.wait_for_service('/stow_the_robot') rospy.wait_for_service('/stop_the_robot') rospy.loginfo('Node ' + self.node_name + ' connected to robot services.') - self.home_the_robot_service = rospy.ServiceProxy('/home_the_robot', Trigger) - self.stow_the_robot_service = rospy.ServiceProxy('/stow_the_robot', Trigger) - self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger) + if self.robot_name == None: + self.home_the_robot_service = rospy.ServiceProxy('/home_the_robot', Trigger) + self.stow_the_robot_service = rospy.ServiceProxy('/stow_the_robot', Trigger) + self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger) + else: + self.home_the_robot_service = rospy.ServiceProxy('/'+self.robot_name+'/home_the_robot', Trigger) + self.stow_the_robot_service = rospy.ServiceProxy('/'+self.robot_name+'/stow_the_robot', Trigger) + self.stop_the_robot_service = rospy.ServiceProxy('/'+self.robot_name+'/stop_the_robot', Trigger) if wait_for_first_pointcloud: # Do not start until a point cloud has been received diff --git a/stretch_core/launch/stretch_driver.launch b/stretch_core/launch/stretch_driver.launch index cc80f7a..6b81dc8 100644 --- a/stretch_core/launch/stretch_driver.launch +++ b/stretch_core/launch/stretch_driver.launch @@ -2,6 +2,8 @@ + + - + diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 7c5c482..28ff654 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/stretch_core/nodes/joint_trajectory_server.py @@ -19,7 +19,11 @@ class JointTrajectoryAction: def __init__(self, node): self.node = node - self.server = actionlib.SimpleActionServer('/stretch_controller/follow_joint_trajectory', + if self.node.robot_name==None: + servername = '/stretch_controller/follow_joint_trajectory' + else: + servername = '/'+self.node.robot_name+'/follow_joint_trajectory' + self.server = actionlib.SimpleActionServer(servername, FollowJointTrajectoryAction, execute_cb=self.execute_cb, auto_start=False) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 151ceed..251694e 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -2,10 +2,12 @@ import yaml import copy +import argparse import numpy as np import threading from rwlock import RWLock import stretch_body.robot as rb +import stretch_body.hello_utils as hu from stretch_body.hello_utils import ThreadServiceExit import stretch_body @@ -34,7 +36,7 @@ from stretch_diagnostics import StretchDiagnostics class StretchDriverNode: - def __init__(self): + def __init__(self, robot_name = None): self.default_goal_timeout_s = 10.0 self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s) @@ -48,6 +50,8 @@ class StretchDriverNode: self.voltage_history = [] self.charging_state_history = [BatteryState.POWER_SUPPLY_STATUS_UNKNOWN] * 10 self.charging_state = BatteryState.POWER_SUPPLY_STATUS_UNKNOWN + + self.robot_name = robot_name ###### MOBILE BASE VELOCITY METHODS ####### @@ -538,7 +542,10 @@ class StretchDriverNode: self.odom_frame_id = 'odom' rospy.loginfo("{0} odom_frame_id = {1}".format(self.node_name, self.odom_frame_id)) - self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) + if self.robot_name==None: + self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1) + else: + self.joint_state_pub = rospy.Publisher('/'+self.robot_name+'joint_states', JointState, queue_size=1) command_base_velocity_and_publish_joint_state_rate = rospy.Rate(self.joint_state_rate) self.last_twist_time = rospy.get_time() @@ -594,5 +601,10 @@ class StretchDriverNode: if __name__ == '__main__': - node = StretchDriverNode() + parser = argparse.ArgumentParser() + parser.add_argument('--enable_multi_robot', action='store_true', help='Turn on namespacing of topics/services/actions to the robots unique name, thereby allowing control of multiple robots simultaneously.') + + args, unknown = parser.parse_known_args() + robot_name = hu.get_fleet_id() if args.enable_multi_robot else None + node = StretchDriverNode(robot_name) node.main() From a937047fee5371d0b074cba65cb78e495f7090f5 Mon Sep 17 00:00:00 2001 From: yy389 Date: Tue, 10 Oct 2023 22:14:25 -0400 Subject: [PATCH 3/4] clean if-else --- hello_helpers/src/hello_helpers/hello_misc.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 5d0d944..b3fb0dd 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -240,19 +240,16 @@ class HelloNode: if not server_reached: rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.') sys.exit() - - if self.robot_name == None: - self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback) - else: - self._joint_states_subscriber = rospy.Subscriber('/'+self.robot_name+'/joint_states', JointState, self._joint_states_callback) - + self.tf2_buffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) if self.robot_name == None: + self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback) self._tool_subscriber = rospy.Subscriber('/tool', String, self._tool_callback) self._mode_subscriber = rospy.Subscriber('/mode', String, self._mode_callback) self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self._point_cloud_callback) else: + self._joint_states_subscriber = rospy.Subscriber('/'+self.robot_name+'/joint_states', JointState, self._joint_states_callback) self._tool_subscriber = rospy.Subscriber('/'+self.robot_name+'/tool', String, self._tool_callback) self._mode_subscriber = rospy.Subscriber('/'+self.robot_name+'/mode', String, self._mode_callback) self.point_cloud_subscriber = rospy.Subscriber('/'+self.robot_name+'/camera/depth/color/points', PointCloud2, self._point_cloud_callback) From 9566763ae8278215d12903dddf5205e3d3152d6c Mon Sep 17 00:00:00 2001 From: yy389 Date: Tue, 10 Oct 2023 22:22:49 -0400 Subject: [PATCH 4/4] clean if-else Co-authored-by: Janna Lin --- hello_helpers/src/hello_helpers/hello_misc.py | 1 + 1 file changed, 1 insertion(+) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index b3fb0dd..eec3603 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -243,6 +243,7 @@ class HelloNode: self.tf2_buffer = tf2_ros.Buffer() self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) + if self.robot_name == None: self._joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self._joint_states_callback) self._tool_subscriber = rospy.Subscriber('/tool', String, self._tool_callback)