Add stow_the_robot service to stretch_driver

stretch_core/nodes/joint_trajectory_server.py

@@ -53,6 +53,10 @@ class JointTrajectoryAction:
             # Escape stopped mode to execute trajectory
             self.node.stop_the_robot = False
         self.node.robot_mode_rwlock.acquire_read()
+        if self.node.robot_mode not in ['position', 'manipulation', 'navigation']:
+            self.invalid_goal_callback("Cannot execute goals while in mode={0}".format(self.node.robot_mode))
+            self.node.robot_mode_rwlock.release_read()
+            return
 
         # For now, ignore goal time and configuration tolerances.
         commanded_joint_names = goal.trajectory.joint_names

stretch_core/nodes/stretch_driver

@@ -1,6 +1,7 @@
 #! /usr/bin/env python3
 
 import yaml
+import copy
 import numpy as np
 import threading
 from rwlock import RWLock
@@ -291,6 +292,16 @@ class StretchDriverNode:
             self.robot.home()
         self.change_mode('calibration', code_to_run)
 
+    def stow_the_robot(self):
+        self.robot_mode_rwlock.acquire_read()
+        last_robot_mode = copy.copy(self.robot_mode)
+        self.robot_mode_rwlock.release_read()
+        def code_to_run():
+            pass
+        self.change_mode('stowing', code_to_run)
+        self.robot.stow()
+        self.change_mode(last_robot_mode, code_to_run)
+
     ######## SERVICE CALLBACKS #######
 
     def stop_the_robot_callback(self, request):
@@ -322,6 +333,14 @@ class StretchDriverNode:
             message='Calibrated.'
         )
 
+    def stow_the_robot_callback(self, request):
+        rospy.loginfo('Recevied stow_the_robot service call.')
+        self.stow_the_robot()
+        return TriggerResponse(
+            success=True,
+            message='Stowed.'
+        )
+
     def navigation_mode_service_callback(self, request):
         self.turn_on_navigation_mode()
         return TriggerResponse(
@@ -484,6 +503,10 @@ class StretchDriverNode:
                                                          Trigger,
                                                          self.calibrate_callback)
 
+        self.stow_the_robot_service = rospy.Service('/stow_the_robot',
+                                                    Trigger,
+                                                    self.stow_the_robot_callback)
+
         self.runstop_service = rospy.Service('/runstop',
                                               SetBool,
                                               self.runstop_service_callback)