diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index 076f48c..20f25ce 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/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(): diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop index e7f2bc8..412948c 100755 --- a/stretch_core/nodes/keyboard_teleop +++ b/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) diff --git a/stretch_demos/nodes/clean_surface b/stretch_demos/nodes/clean_surface index 97cf300..ad33e79 100755 --- a/stretch_demos/nodes/clean_surface +++ b/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) diff --git a/stretch_demos/nodes/grasp_object b/stretch_demos/nodes/grasp_object index cda9b9f..ebd9ccb 100755 --- a/stretch_demos/nodes/grasp_object +++ b/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) diff --git a/stretch_demos/nodes/hello_world b/stretch_demos/nodes/hello_world index d1eac35..da68b64 100755 --- a/stretch_demos/nodes/hello_world +++ b/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) diff --git a/stretch_demos/nodes/open_drawer b/stretch_demos/nodes/open_drawer index 412cb1f..652126c 100755 --- a/stretch_demos/nodes/open_drawer +++ b/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) diff --git a/stretch_funmap/nodes/funmap b/stretch_funmap/nodes/funmap index ab817d7..d3d2a6b 100755 --- a/stretch_funmap/nodes/funmap +++ b/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) diff --git a/stretch_gazebo/scripts/publish_ground_truth_odom.py b/stretch_gazebo/scripts/publish_ground_truth_odom.py index 220e3be..568a6da 100755 --- a/stretch_gazebo/scripts/publish_ground_truth_odom.py +++ b/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)