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