Added template for dock_robot node

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,40 @@
+<launch>
+
+  <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'/>
+  <!-- -->
+
+</launch>

stretch_demos/nodes/dock_robot

@@ -0,0 +1,55 @@
+#!/usr/bin/env python3
+
+import rospy
+from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
+
+import argparse as ap
+import hello_helpers.hello_misc as hm
+
+
+class DockRobotNode(hm.HelloNode):
+
+    def __init__(self):
+        hm.HelloNode.__init__(self)
+        self.rate = 10.0
+        self.debug_directory = None
+        
+    def trigger_dock_robot_callback(self, request):
+        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')
+        rospy.loginfo('Using the following directory for debugging files: {0}'.format(self.debug_directory))
+
+        self.trigger_grasp_object_service = rospy.Service('/dock_robot/trigger_dock_robot',
+                                                          Trigger,
+                                                          self.trigger_dock_robot_callback)
+
+        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.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)
+        rospy.wait_for_service(high_accuracy_service)
+        rospy.loginfo('Node ' + self.node_name + ' connected to'  + high_accuracy_service)
+        self.trigger_d435i_high_accuracy_mode_service = rospy.ServiceProxy(high_accuracy_service, Trigger)
+        
+        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')