@@ -240,19 +240,16 @@ def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
240240 if not server_reached :
241241 rospy .signal_shutdown ('Unable to connect to arm action server. Timeout exceeded.' )
242242 sys .exit ()
243-
244- if self .robot_name == None :
245- self ._joint_states_subscriber = rospy .Subscriber ('/stretch/joint_states' , JointState , self ._joint_states_callback )
246- else :
247- self ._joint_states_subscriber = rospy .Subscriber ('/' + self .robot_name + '/joint_states' , JointState , self ._joint_states_callback )
248-
243+
249244 self .tf2_buffer = tf2_ros .Buffer ()
250245 self .tf2_listener = tf2_ros .TransformListener (self .tf2_buffer )
251246 if self .robot_name == None :
247+ self ._joint_states_subscriber = rospy .Subscriber ('/stretch/joint_states' , JointState , self ._joint_states_callback )
252248 self ._tool_subscriber = rospy .Subscriber ('/tool' , String , self ._tool_callback )
253249 self ._mode_subscriber = rospy .Subscriber ('/mode' , String , self ._mode_callback )
254250 self .point_cloud_subscriber = rospy .Subscriber ('/camera/depth/color/points' , PointCloud2 , self ._point_cloud_callback )
255251 else :
252+ self ._joint_states_subscriber = rospy .Subscriber ('/' + self .robot_name + '/joint_states' , JointState , self ._joint_states_callback )
256253 self ._tool_subscriber = rospy .Subscriber ('/' + self .robot_name + '/tool' , String , self ._tool_callback )
257254 self ._mode_subscriber = rospy .Subscriber ('/' + self .robot_name + '/mode' , String , self ._mode_callback )
258255 self .point_cloud_subscriber = rospy .Subscriber ('/' + self .robot_name + '/camera/depth/color/points' , PointCloud2 , self ._point_cloud_callback )
0 commit comments