Log info before waiting for ROS service

hello_helpers/src/hello_helpers/hello_misc.py

@@ -142,17 +142,20 @@ class HelloNode:
         rospy.loginfo("{0} started".format(self.node_name))
 
         self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /stretch_controller/follow_joint_trajectory server.')
         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.')
+            rospy.signal_shutdown('Unable to connect to /stretch_controller/follow_joint_trajectory server. Timeout exceeded.')
             sys.exit()
-        
+        rospy.loginfo('Node ' + self.node_name + ' connected to /stretch_controller/follow_joint_trajectory server.')
+
         self.tf2_buffer = tf2_ros.Buffer()
         self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
         
         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.loginfo('Node ' + self.node_name + ' waiting to connect to /stop_the_robot service.')
         rospy.wait_for_service('/stop_the_robot')
         rospy.loginfo('Node ' + self.node_name + ' connected to /stop_the_robot service.')
         self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger)
@@ -160,11 +163,11 @@ class HelloNode:
         if wait_for_first_pointcloud:
             # Do not start until a point cloud has been received
             point_cloud_msg = self.point_cloud
-            print('Node ' + node_name + ' waiting to receive first point cloud.')
+            rospy.loginfo('Node ' + node_name + ' waiting to receive first point cloud.')
             while point_cloud_msg is None:
                 rospy.sleep(0.1)
                 point_cloud_msg = self.point_cloud
-            print('Node ' + node_name + ' received first point cloud, so continuing.')
+            rospy.loginfo('Node ' + node_name + ' received first point cloud, so continuing.')
 
 
 def create_time_string():

stretch_core/nodes/keyboard_teleop

@@ -313,63 +313,73 @@ class KeyboardTeleopNode(hm.HelloNode):
     def main(self):
         hm.HelloNode.main(self, 'keyboard_teleop', 'keyboard_teleop', wait_for_first_pointcloud=False)
 
-        if self.mapping_on: 
+        if self.mapping_on:
             rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_head_scan.')
-
             rospy.wait_for_service('/funmap/trigger_head_scan')
             rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_head_scan.')
             self.trigger_head_scan_service = rospy.ServiceProxy('/funmap/trigger_head_scan', Trigger)
 
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_drive_to_scan.')
             rospy.wait_for_service('/funmap/trigger_drive_to_scan')
             rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_drive_to_scan.')
             self.trigger_drive_to_scan_service = rospy.ServiceProxy('/funmap/trigger_drive_to_scan', Trigger)
 
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_global_localization.')
             rospy.wait_for_service('/funmap/trigger_global_localization')
             rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_global_localization.')
             self.trigger_global_localization_service = rospy.ServiceProxy('/funmap/trigger_global_localization', Trigger)
 
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_local_localization.')
             rospy.wait_for_service('/funmap/trigger_local_localization')
             rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_local_localization.')
             self.trigger_local_localization_service = rospy.ServiceProxy('/funmap/trigger_local_localization', Trigger)
 
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_align_with_nearest_cliff.')
             rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
             rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
             self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
 
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_reach_until_contact.')
             rospy.wait_for_service('/funmap/trigger_reach_until_contact')
             rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.')
             self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger)
 
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_lower_until_contact.')
             rospy.wait_for_service('/funmap/trigger_lower_until_contact')
             rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
             self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)
 
-        if self.hello_world_on: 
+        if self.hello_world_on:
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /hello_world/trigger_write_hello.')
             rospy.wait_for_service('/hello_world/trigger_write_hello')
             rospy.loginfo('Node ' + self.node_name + ' connected to /hello_world/trigger_write_hello.')
             self.trigger_write_hello_service = rospy.ServiceProxy('/hello_world/trigger_write_hello', Trigger)
 
         if self.open_drawer_on:
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /open_drawer/trigger_open_drawer_down.')
             rospy.wait_for_service('/open_drawer/trigger_open_drawer_down')
             rospy.loginfo('Node ' + self.node_name + ' connected to /open_drawer/trigger_open_drawer_down.')
             self.trigger_open_drawer_down_service = rospy.ServiceProxy('/open_drawer/trigger_open_drawer_down', Trigger)
 
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /open_drawer/trigger_open_drawer_up.')
             rospy.wait_for_service('/open_drawer/trigger_open_drawer_up')
             rospy.loginfo('Node ' + self.node_name + ' connected to /open_drawer/trigger_open_drawer_up.')
             self.trigger_open_drawer_up_service = rospy.ServiceProxy('/open_drawer/trigger_open_drawer_up', Trigger)
 
-            
         if self.clean_surface_on:
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /clean_surface/trigger_clean_surface.')
             rospy.wait_for_service('/clean_surface/trigger_clean_surface')
             rospy.loginfo('Node ' + self.node_name + ' connected to /clean_surface/trigger_clean_surface.')
             self.trigger_clean_surface_service = rospy.ServiceProxy('/clean_surface/trigger_clean_surface', Trigger)
 
         if self.grasp_object_on:
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /grasp_object/trigger_grasp_object.')
             rospy.wait_for_service('/grasp_object/trigger_grasp_object')
             rospy.loginfo('Node ' + self.node_name + ' connected to /grasp_object/trigger_grasp_object.')
             self.trigger_grasp_object_service = rospy.ServiceProxy('/grasp_object/trigger_grasp_object', Trigger)
 
         if self.deliver_object_on:
+            rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /deliver_object/trigger_deliver_object.')
             rospy.wait_for_service('/deliver_object/trigger_deliver_object')
             rospy.loginfo('Node ' + self.node_name + ' connected to /deliver_object/trigger_deliver_object.')
             self.trigger_deliver_object_service = rospy.ServiceProxy('/deliver_object/trigger_deliver_object', Trigger)

stretch_demos/nodes/clean_surface

@@ -217,17 +217,19 @@ class CleanSurfaceNode(hm.HelloNode):
                                                            Trigger,
                                                            self.trigger_clean_surface_callback)
 
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_reach_until_contact.')
         rospy.wait_for_service('/funmap/trigger_reach_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.')
         self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger)
 
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_lower_until_contact.')
         rospy.wait_for_service('/funmap/trigger_lower_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
         self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)
 
         default_service = '/camera/switch_to_default_mode'
         high_accuracy_service = '/camera/switch_to_high_accuracy_mode'
-        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service)
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service + ' services.')
         rospy.wait_for_service(default_service)
         rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service)
         self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger)

stretch_demos/nodes/grasp_object

@@ -238,17 +238,19 @@ class GraspObjectNode(hm.HelloNode):
                                                            Trigger,
                                                            self.trigger_grasp_object_callback)
 
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_reach_until_contact.')
         rospy.wait_for_service('/funmap/trigger_reach_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.')
         self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger)
 
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_lower_until_contact.')
         rospy.wait_for_service('/funmap/trigger_lower_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
         self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)
 
         default_service = '/camera/switch_to_default_mode'
         high_accuracy_service = '/camera/switch_to_high_accuracy_mode'
-        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service)
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service + ' services.')
         rospy.wait_for_service(default_service)
         rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service)
         self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger)

stretch_demos/nodes/hello_world

@@ -255,15 +255,18 @@ class HelloWorldNode(hm.HelloNode):
         self.trigger_write_hello_service = rospy.Service('/hello_world/trigger_write_hello',
                                                          Trigger,
                                                          self.trigger_write_hello_callback)
-        
+
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_align_with_nearest_cliff.')
         rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
         self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
 
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_reach_until_contact.')
         rospy.wait_for_service('/funmap/trigger_reach_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.')
         self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger)
 
+        # rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_turn_off_contact_regulation.')
         # rospy.wait_for_service('/funmap/trigger_turn_off_contact_regulation')
         # rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_turn_off_contact_regulation.')
         # self.trigger_turn_off_contact_regulation_service = rospy.ServiceProxy('/funmap/trigger_turn_off_contact_regulation', Trigger)

stretch_demos/nodes/open_drawer

@@ -199,15 +199,17 @@ class OpenDrawerNode(hm.HelloNode):
                                                          Trigger,
                                                          self.trigger_open_drawer_up_callback)
         
-        
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_align_with_nearest_cliff.')
         rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
         self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
 
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_reach_until_contact.')
         rospy.wait_for_service('/funmap/trigger_reach_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_reach_until_contact.')
         self.trigger_reach_until_contact_service = rospy.ServiceProxy('/funmap/trigger_reach_until_contact', Trigger)
 
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_lower_until_contact.')
         rospy.wait_for_service('/funmap/trigger_lower_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
         self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)

stretch_funmap/nodes/funmap

@@ -1264,7 +1264,7 @@ class FunmapNode(hm.HelloNode):
         
         default_service = '/camera/switch_to_default_mode'
         high_accuracy_service = '/camera/switch_to_high_accuracy_mode'
-        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service)
+        rospy.loginfo('Node ' + self.node_name + ' waiting to connect to ' + default_service + ' and ' + high_accuracy_service + ' services.')
         rospy.wait_for_service(default_service)
         rospy.loginfo('Node ' + self.node_name + ' connected to ' + default_service)
         self.trigger_d435i_default_mode_service = rospy.ServiceProxy(default_service, Trigger)

stretch_gazebo/scripts/publish_ground_truth_odom.py

@@ -10,6 +10,7 @@ import time
 rospy.init_node('ground_truth_odometry_publisher')
 odom_pub=rospy.Publisher('ground_truth', Odometry, queue_size=10)
 
+rospy.loginfo('Waiting to connect to /gazebo/get_model_state.')
 rospy.wait_for_service('/gazebo/get_model_state')
 get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
 get_world_properties = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)