cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Log info before waiting for ROS service

feature/funmap_simulation_updates
Binit Shah 2 years ago
parent
59a0b82049
commit
4a3f84fdc4
8 changed files with 36 additions and 13 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +7
    -4
      hello_helpers/src/hello_helpers/hello_misc.py
  2. +14
    -4
      stretch_core/nodes/keyboard_teleop
  3. +3
    -1
      stretch_demos/nodes/clean_surface
  4. +3
    -1
      stretch_demos/nodes/grasp_object
  5. +4
    -1
      stretch_demos/nodes/hello_world
  6. +3
    -1
      stretch_demos/nodes/open_drawer
  7. +1
    -1
      stretch_funmap/nodes/funmap
  8. +1
    -0
      stretch_gazebo/scripts/publish_ground_truth_odom.py

+ 7
- 4
hello_helpers/src/hello_helpers/hello_misc.py View File

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

+ 14
- 4
stretch_core/nodes/keyboard_teleop View File

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

+ 3
- 1
stretch_demos/nodes/clean_surface View File

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

+ 3
- 1
stretch_demos/nodes/grasp_object View File

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

+ 4
- 1
stretch_demos/nodes/hello_world View File

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

+ 3
- 1
stretch_demos/nodes/open_drawer View File

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

+ 1
- 1
stretch_funmap/nodes/funmap View File

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

+ 1
- 0
stretch_gazebo/scripts/publish_ground_truth_odom.py View File

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

Write Preview
Loading…
Cancel
Save