From 74f3eb4a623f5a632100c1f621dc855315c0c8cc Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Sat, 5 Nov 2022 21:44:58 -0700 Subject: [PATCH 1/5] Add stow_the_robot service to stretch_driver --- stretch_core/nodes/joint_trajectory_server.py | 4 ++++ stretch_core/nodes/stretch_driver | 23 +++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/nodes/joint_trajectory_server.py index 7a195fa..8273290 100644 --- a/stretch_core/nodes/joint_trajectory_server.py +++ b/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 diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 08af23d..22aa2e1 100755 --- a/stretch_core/nodes/stretch_driver +++ b/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) From 66d88ceba3d8f98efc661fe30db6f3314032999c Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Sat, 5 Nov 2022 21:54:00 -0700 Subject: [PATCH 2/5] Fix calibrate_the_robot service threading bug --- stretch_core/nodes/stretch_driver | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver index 22aa2e1..145093a 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/nodes/stretch_driver @@ -288,9 +288,14 @@ 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() From f677bee879040166bcbbb3dd4bfe4518d927231a Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Sat, 5 Nov 2022 22:21:39 -0700 Subject: [PATCH 3/5] Add API docs for calibrate/stow_the_robot services --- stretch_core/README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/stretch_core/README.md b/stretch_core/README.md index 5972aaa..c73fb27 100644 --- a/stretch_core/README.md +++ b/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 one-time 30-second procedure that must occur everytime the robot wakes up before you may send motion commands to or read correct joint positions from any of Stretch's 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 stow 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: From 0e70ccd2d4fc29dc7d4dfce603ea84ac02f4e8ee Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Sun, 6 Nov 2022 11:13:06 -0800 Subject: [PATCH 4/5] Fix typos in calibrate/stow_the_robot service API docs --- stretch_core/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/stretch_core/README.md b/stretch_core/README.md index c73fb27..e91a74f 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -30,13 +30,13 @@ Since a battery is always present on a Stretch system, we instead misuse the `pr ##### /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 one-time 30-second procedure that must occur everytime the robot wakes up before you may send motion commands to or read correct joint positions from any of Stretch's 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. +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 one-time 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 stow these additional joints automatically. +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. From e9ebb63b3cae1eda124cae4db10b47a884ede727 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Sun, 6 Nov 2022 11:15:09 -0800 Subject: [PATCH 5/5] Fix phrasing in calibrate_the_robot service API docs --- stretch_core/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/stretch_core/README.md b/stretch_core/README.md index e91a74f..9f14c6d 100644 --- a/stretch_core/README.md +++ b/stretch_core/README.md @@ -30,7 +30,7 @@ Since a battery is always present on a Stretch system, we instead misuse the `pr ##### /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 one-time 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. +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.