Merge pull request #83 from hello-robot/feature/stow_service

Add stow robot service and fix home robot service in stretch_driver

stretch_core/README.md

@@ -26,6 +26,20 @@ This topic publishes Stretch's battery and charge status. Charging status, the `
 
 Since a battery is always present on a Stretch system, we instead misuse the `present` field to indicate whether a plug is plugged in to the charging port (regardless of whether it's providing power) on RE2 robots. This field is always false on RE1s. The unmeasured fields (e.g. charge in Ah) return a NaN, or 'not a number'.
 
+#### Published Services
+
+##### /calibrate_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
+
+This service will start Stretch's homing procedure, where every joint's zero is found. Robots with relative encoders (vs absolute encoders) need a homing procedure when they power on. For Stretch, it's a 30-second procedure that must occur everytime the robot wakes up before you may send motion commands to or read correct joint positions from Stretch's prismatic and multiturn revolute joints. When this service is triggered, the [mode topic](#mode-std_msgsstring) will reflect that the robot is in "calibration" mode, and after the homing procedure is complete, will switch back to whatever mode the robot was in before this service was triggered. While stretch_driver is in "calibration" mode, no commands to the [cmd_vel topic](#TODO) or the [follow joint trajectory action service](#TODO) will be accepted.
+
+Other ways to home the robot include using the `stretch_robot_home.py` CLI tool from a terminal, or calling [`robot.home()`](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_stretch_body_api/#stretch_body.robot.Robot.home) from Stretch's Python API.
+
+##### /stow_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
+
+This service will start Stretch's stowing procedure, where the arm is stowed into the footprint of the mobile base. This service is more convenient than sending a [follow joint trajectory command](#TODO) since it knows what gripper is installed at the end of arm and stows these additional joints automatically.
+
+Other ways to stow the robot include using the `stretch_robot_stow.py` CLI tool from a terminal, or calling [`robot.stow()`](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_stretch_body_api/#stretch_body.robot.Robot.stow) from Stretch's Python API.
+
 # Testing
 
 The primary testing framework being used within *stretch_ros* is pytest. Pytest is an open source testing framework that scales well and takes a functional approach resulting in minimal boiler plate code. First we should ensure that pytest is installed and up to date:

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
@@ -287,9 +288,24 @@ class StretchDriverNode:
         self.change_mode('position', code_to_run)
 
     def calibrate(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():
-            self.robot.home()
+            pass
         self.change_mode('calibration', code_to_run)
+        self.robot.home()
+        self.change_mode(last_robot_mode, 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 #######
 
@@ -322,6 +338,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 +508,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)