multirobot revision 1

Author: tenny-yinyijun

hello_helpers/src/hello_helpers/hello_misc.py

@@ -241,24 +241,36 @@ def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
             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

stretch_core/launch/stretch_driver.launch

@@ -2,6 +2,8 @@
 
   <param name="robot_description" textfile="$(find stretch_description)/urdf/stretch.urdf" />
   <arg name="calibrated_controller_yaml_file" value="$(find stretch_core)/config/controller_calibration_head.yaml"/>
+  <arg name="enable_multi_robot" default="false" doc="Turn on namespacing of topics/services/actions to the robots unique name, thereby allowing control of multiple robots simultaneously." />
+  <arg name="__enable_multi_robot_arg" value="$(eval '--enable_multi_robot' if arg('enable_multi_robot') else '')" />
 
   <node
       name="joint_state_publisher"
@@ -33,7 +35,7 @@
     <param name="publish_frequency" value="30.0"/>
   </node>
 
-  <node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen">
+  <node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen" args='$(arg __enable_multi_robot_arg)'>
     <param name="rate" type="double" value="30.0"/>
     <param name="timeout" type="double" value="0.5"/>
     <remap from="cmd_vel" to="/stretch/cmd_vel" />

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)

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()