Automatic docking for Stretch

hello_helpers/src/hello_helpers/hello_misc.py

@@ -140,32 +140,38 @@ def get_robot_floor_pose_xya(self, floor_frame='odom'):
     def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
         rospy.init_node(node_name)
         self.node_name = rospy.get_name()
-        rospy.loginfo("{0} started".format(self.node_name))
+        rospy.loginfo(f"{self.node_name} started")
 
-        self.trajectory_client = actionlib.SimpleActionClient('/stretch_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
+        fjt_actionserver_name = '/stretch_controller/follow_joint_trajectory'
+        rospy.logdebug(f'{self.node_name} waiting to connect to action server {fjt_actionserver_name}')
+        self.trajectory_client = actionlib.SimpleActionClient(fjt_actionserver_name, FollowJointTrajectoryAction)
         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.logerr(f'{self.node_name} unable to connect to robot action server. Timeout exceeded.')
+            rospy.signal_shutdown('Unable to connect to robot action server. Timeout exceeded.')
             sys.exit()
-
+        rospy.logdebug(f'{self.node_name} connected to {fjt_actionserver_name}')
+
         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.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)
-
+        stop_robot_service_name = '/stop_the_robot'
+        rospy.logdebug(f'{self.node_name} waiting to connect to service {stop_robot_service_name}')
+        rospy.wait_for_service(stop_robot_service_name)
+        self.stop_the_robot_service = rospy.ServiceProxy(stop_robot_service_name, Trigger)
+        rospy.logdebug(f'{self.node_name} connected to {stop_robot_service_name}')
+
         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.logdebug(f'{self.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.logdebug(f'{self.node_name} received first point cloud, so continuing')
 
 
 def create_time_string():

stretch_core/config/stretch_marker_dict.yaml

@@ -37,6 +37,11 @@
     'use_rgb_only': False
     'name': 'shoulder_top'
     'link': 'link_aruco_shoulder'
+  '245':
+    'length_mm': 88.0
+    'use_rgb_only': False
+    'name': 'link_docking_station'
+    'link': None
   '246':
     'length_mm': 179.0
     'use_rgb_only': False

stretch_core/nodes/keyboard_teleop

@@ -15,13 +15,14 @@ import hello_helpers.hello_misc as hm
 
 class GetKeyboardCommands:
 
-    def __init__(self, mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on):
+    def __init__(self, mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on, docking_on):
         self.mapping_on = mapping_on
         self.hello_world_on = hello_world_on
         self.open_drawer_on = open_drawer_on
         self.clean_surface_on = clean_surface_on
         self.grasp_object_on = grasp_object_on
         self.deliver_object_on = deliver_object_on
+        self.docking_on = docking_on
 
         self.step_size = 'medium'
         self.rad_per_deg = math.pi/180.0
@@ -210,6 +211,11 @@ class GetKeyboardCommands:
             trigger_result = node.trigger_deliver_object_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
+        if ((c == 'n') or (c == 'N')) and self.docking_on:
+            trigger_request = TriggerRequest()
+            trigger_result = node.trigger_dock_robot_service(trigger_request)
+            rospy.loginfo('trigger_result = {0}'.format(trigger_result))
+
 
         ####################################################
         ## BASIC KEYBOARD TELEOPERATION COMMANDS
@@ -294,9 +300,9 @@ class GetKeyboardCommands:
 
 class KeyboardTeleopNode(hm.HelloNode):
 
-    def __init__(self, mapping_on=False, hello_world_on=False, open_drawer_on=False, clean_surface_on=False, grasp_object_on=False, deliver_object_on=False):
+    def __init__(self, mapping_on=False, hello_world_on=False, open_drawer_on=False, clean_surface_on=False, grasp_object_on=False, deliver_object_on=False, docking_on=False):
         hm.HelloNode.__init__(self)
-        self.keys = GetKeyboardCommands(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on)
+        self.keys = GetKeyboardCommands(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on, docking_on)
         self.rate = 10.0
         self.joint_state = None
         self.mapping_on = mapping_on
@@ -305,6 +311,7 @@ class KeyboardTeleopNode(hm.HelloNode):
         self.clean_surface_on = clean_surface_on
         self.grasp_object_on = grasp_object_on
         self.deliver_object_on = deliver_object_on
+        self.docking_on = docking_on
 
     def joint_states_callback(self, joint_state):
         self.joint_state = joint_state
@@ -396,6 +403,11 @@ class KeyboardTeleopNode(hm.HelloNode):
             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)
 
+        if self.docking_on:
+            rospy.wait_for_service('/dock_robot/trigger_dock_robot')
+            rospy.loginfo('Node ' + self.node_name + ' connected to /dock_robot/trigger_dock_robot.')
+            self.trigger_dock_robot_service = rospy.ServiceProxy('/dock_robot/trigger_dock_robot', Trigger)
+
         rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
 
         rate = rospy.Rate(self.rate)
@@ -418,6 +430,7 @@ if __name__ == '__main__':
         parser.add_argument('--clean_surface_on', action='store_true', help='Enable Clean Surface trigger, which requires connection to the appropriate clean_surface service.')
         parser.add_argument('--grasp_object_on', action='store_true', help='Enable Grasp Object trigger, which requires connection to the appropriate grasp_object service.')
         parser.add_argument('--deliver_object_on', action='store_true', help='Enable Deliver Object trigger, which requires connection to the appropriate deliver_object service.')
+        parser.add_argument('--docking_on', action='store_true', help='Enable Dock Robot trigger, which requires connection to the appropriate dock_robot service.')
 
         args, unknown = parser.parse_known_args()
         mapping_on = args.mapping_on
@@ -426,8 +439,9 @@ if __name__ == '__main__':
         clean_surface_on = args.clean_surface_on
         grasp_object_on = args.grasp_object_on
         deliver_object_on = args.deliver_object_on
+        docking_on = args.docking_on
 
-        node = KeyboardTeleopNode(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on)
+        node = KeyboardTeleopNode(mapping_on, hello_world_on, open_drawer_on, clean_surface_on, grasp_object_on, deliver_object_on, docking_on)
         node.main()
     except KeyboardInterrupt:
         rospy.loginfo('interrupt received, so shutting down')

stretch_demos/launch/dock_robot.launch

@@ -0,0 +1,45 @@
+<launch>
+
+  <arg name="rviz" default="false" doc="whether to show Rviz" />
+  <arg name="debug_directory" value="$(env HELLO_FLEET_PATH)/debug/"/>
+
+  <!-- REALSENSE D435i -->
+  <include file="$(find stretch_core)/launch/d435i_high_resolution.launch" />
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen" />
+  <!-- -->
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
+  <!-- -->
+
+  <!-- MAPPING -->
+  <node name="funmap" pkg="stretch_funmap" type="funmap" output="screen" >
+    <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
+  </node>
+  <!-- -->
+
+  <!-- LASER RANGE FINDER -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" />
+  <!-- -->
+
+  <!-- LASER SCAN MATCHER FOR ODOMETRY -->
+  <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" />
+  <!-- -->
+
+  <!-- DOCK ROBOT -->
+  <node name="dock_robot" pkg="stretch_demos" type="dock_robot" output="screen">
+    <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
+  </node>
+  <!-- -->
+
+  <!-- KEYBOARD TELEOP -->
+  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on --docking_on'/>
+  <!-- -->
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_demos)/rviz/dock_robot.rviz" if="$(arg rviz)" />
+  <!-- -->
+
+</launch>

stretch_demos/nodes/dock_robot

@@ -0,0 +1,117 @@
+#!/usr/bin/env python3
+
+import rospy
+from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
+from sensor_msgs.msg import BatteryState
+
+import copy
+import threading
+import argparse as ap
+import hello_helpers.hello_misc as hm
+import stretch_body.hello_utils as hu
+
+
+class DockRobotNode(hm.HelloNode):
+
+    def __init__(self):
+        hm.HelloNode.__init__(self)
+        self.rate = 10.0
+        self.debug_directory = None
+        self.battery_state = None
+        self.battery_state_lock = threading.Lock()
+
+    def battery_state_callback(self, battery_state):
+        with self.battery_state_lock:
+            self.battery_state = battery_state
+
+    def trigger_dock_robot_callback(self, request):
+        # 1. Check predocking conditions (e.g. not already docked, arm stowed, etc.)
+        with self.battery_state_lock:
+            battery_state = copy.copy(self.battery_state)
+        rospy.logdebug(f'{self.node_name} got battery_state={battery_state}')
+        if battery_state.present:
+            rospy.logerr('Already plugged into charger')
+            return TriggerResponse(
+                success=False,
+                message='Already plugged into charger'
+            )
+        if battery_state.power_supply_status == BatteryState.POWER_SUPPLY_STATUS_CHARGING:
+            rospy.logerr('Already charging')
+            return TriggerResponse(
+                success=False,
+                message='Already charging'
+            )
+        stow_trigger_request = TriggerRequest()
+        stow_trigger_result = self.trigger_stow_the_robot_service(stow_trigger_request)
+        if not stow_trigger_result.success:
+            rospy.logerr(f'Robot driver failed to stow arm with msg={stow_trigger_result.message}')
+            return TriggerResponse(
+                success=False,
+                message=stow_trigger_result.message
+            )
+        position_mode_trigger_request = TriggerRequest()
+        position_mode_trigger_result = self.trigger_position_mode_service(position_mode_trigger_request)
+        if not position_mode_trigger_result.success:
+            rospy.logerr(f'Robot driver failed to switch into position mode with msg={position_mode_trigger_result.message}')
+            return TriggerResponse(
+                success=False,
+                message=position_mode_trigger_result.message
+            )
+
+        # 2. Perform a head scan, looking for the docking station's ArUco marker
+        # 3. Ask FUNMAP to plan and execute the mobile base to a pose in front of the docking station
+        # 4. Back up until charging detected
+
+        return TriggerResponse(
+            success=False,
+            message='Not Implemented'
+        )
+
+    def main(self):
+        hm.HelloNode.main(self, 'dock_robot', 'dock_robot', wait_for_first_pointcloud=False)
+
+        self.debug_directory = rospy.get_param('~debug_directory', hu.get_stretch_directory('debug'))
+        rospy.logdebug(f'Using the following directory for debugging files: {self.debug_directory}')
+
+        self.battery_state_subscriber = rospy.Subscriber('/battery', BatteryState, self.battery_state_callback)
+
+        position_mode_service_name = '/switch_to_position_mode'
+        rospy.logdebug(f'{self.node_name} waiting to connect to service {position_mode_service_name}')
+        rospy.wait_for_service(position_mode_service_name)
+        self.trigger_position_mode_service = rospy.ServiceProxy(position_mode_service_name, Trigger)
+        rospy.logdebug(f'{self.node_name} connected to {position_mode_service_name}')
+
+        stow_service_name = '/stow_the_robot'
+        rospy.logdebug(f'{self.node_name} waiting to connect to service {stow_service_name}')
+        rospy.wait_for_service(stow_service_name)
+        self.trigger_stow_the_robot_service = rospy.ServiceProxy(stow_service_name, Trigger)
+        rospy.logdebug(f'{self.node_name} connected to {stow_service_name}')
+
+        default_camera_preset_service_name = '/camera/switch_to_default_mode'
+        high_accuracy_camera_preset_service_name = '/camera/switch_to_high_accuracy_mode'
+        rospy.logdebug(f'{self.node_name} waiting to connect to services {default_camera_preset_service_name} and {high_accuracy_camera_preset_service_name}')
+        rospy.wait_for_service(default_camera_preset_service_name)
+        self.trigger_d435i_default_preset_service = rospy.ServiceProxy(default_camera_preset_service_name, Trigger)
+        rospy.logdebug(f'{self.node_name} connected to {default_camera_preset_service_name}')
+        rospy.wait_for_service(high_accuracy_camera_preset_service_name)
+        self.trigger_d435i_high_accuracy_preset_service = rospy.ServiceProxy(high_accuracy_camera_preset_service_name, Trigger)
+        rospy.logdebug(f'{self.node_name} connected to {high_accuracy_camera_preset_service_name}')
+
+        self.trigger_grasp_object_service = rospy.Service('/dock_robot/trigger_dock_robot',
+                                                          Trigger,
+                                                          self.trigger_dock_robot_callback)
+
+        rospy.loginfo(f'{self.node_name} ready!')
+        rate = rospy.Rate(self.rate)
+        while not rospy.is_shutdown():
+            rate.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        parser = ap.ArgumentParser(description='Dock Robot behavior for stretch.')
+        args, unknown = parser.parse_known_args()
+        node = DockRobotNode()
+        node.main()
+    except KeyboardInterrupt:
+        rospy.loginfo('interrupt received, so shutting down')