Add dryrun option to grasp_object

hello_helpers/README.md

@@ -57,6 +57,33 @@ temp = hm.HelloNode.quick_create('temp')
 temp.move_to_pose({'joint_lift': 0.4})
 ```
 
+#### Attributes
+
+##### `dryrun`
+
+This attribute allows you to control whether the robot actually moves when calling `move_to_pose()`, `home_the_robot()`, `stow_the_robot()`, or other motion methods in this class. When `dryrun` is set to True, these motion methods return immediately. This attribute is helpful when you want to run just the perception/planning part of your node without actually moving the robot. For example, you could replace the following verbose snippet:
+
+```python
+# roslaunch the stretch launch file beforehand
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+actually_move = False
+[...]
+if actually_move:
+    temp.move_to_pose({'translate_mobile_base': 1.0})
+```
+
+to be more consise:
+
+```python
+# roslaunch the stretch launch file beforehand
+import hello_helpers.hello_misc as hm
+temp = hm.HelloNode.quick_create('temp')
+[...]
+temp.dryrun = True
+temp.move_to_pose({'translate_mobile_base': 1.0})
+```
+
 #### Methods
 
 ##### `move_to_pose(pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False)`
@@ -81,6 +108,18 @@ If you set `custom_full_goal` to True, the dictionary format is string/tuple key
 self.move_to_pose({'joint_arm': (0.5, 0.01, 0.01, 40)}, custom_full_goal=True)
 ```
 
+##### `home_the_robot()`
+
+This is a convenience method to interact with the driver's [`/home_the_robot` service](../stretch_core/README.md#home_the_robot-std_srvstrigger).
+
+##### `stow_the_robot()`
+
+This is a convenience method to interact with the driver's [`/stow_the_robot` service](../stretch_core/README.md#stow_the_robot-std_srvstrigger).
+
+##### `stop_the_robot()`
+
+This is a convenience method to interact with the driver's [`/stop_the_robot` service](../stretch_core/README.md#stop_the_robot-std_srvstrigger).
+
 ##### `get_tf(from_frame, to_frame)`
 
 Use this method to get the transform ([geometry_msgs/TransformStamped](https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html)) between two frames. This method is blocking. For example, this method can do forward kinematics from the base_link to the link between the gripper fingers, link_grasp_center, using:

hello_helpers/src/hello_helpers/hello_misc.py

@@ -71,6 +71,7 @@ class HelloNode:
         self.joint_state = None
         self.point_cloud = None
         self.tool = None
+        self.dryrun = False
 
     @classmethod
     def quick_create(cls, name, wait_for_first_pointcloud=False):
@@ -88,6 +89,9 @@ class HelloNode:
         self.tool = tool_string.data
 
     def move_to_pose(self, pose, return_before_done=False, custom_contact_thresholds=False, custom_full_goal=False):
+        if self.dryrun:
+            return
+
         point = JointTrajectoryPoint()
         point.time_from_start = rospy.Duration(0.0)
         trajectory_goal = FollowJointTrajectoryGoal()
@@ -170,12 +174,18 @@ class HelloNode:
             rate.sleep()
 
     def home_the_robot(self):
+        if self.dryrun:
+            return
+
         trigger_request = TriggerRequest()
         trigger_result = self.home_the_robot_service(trigger_request)
         rospy.logdebug(f"{self.node_name}'s HelloNode.home_the_robot: got message {trigger_result.message}")
         return trigger_result.success
 
     def stow_the_robot(self):
+        if self.dryrun:
+            return
+
         trigger_request = TriggerRequest()
         trigger_result = self.stow_the_robot_service(trigger_request)
         rospy.logdebug(f"{self.node_name}'s HelloNode.stow_the_robot: got message {trigger_result.message}")

stretch_core/README.md

@@ -81,6 +81,10 @@ Other ways to stow the robot include using the `stretch_robot_stow.py` CLI tool
 
 Runstopping the robot while this service is running will yield undefined behavior and likely leave the driver in a bad state.
 
+##### /stop_the_robot ([std_srvs/Trigger](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/Trigger.html))
+
+This service immediately stops any currently active motion.
+
 ##### /runstop ([std_srvs/SetBool](https://docs.ros.org/en/noetic/api/std_srvs/html/srv/SetBool.html))
 
 This service can put Stretch into runstop or take Stretch out of runstop. It's common to put the robot into/out of runstop by pressing the [glowing white button](https://docs.hello-robot.com/0.2/stretch-tutorials/getting_started/safety_guide/#runstop) in Stretch's head (at which point the robot will beep and the button will be blinking to indicate that it's runstopped), and holding the button down for two seconds to take it out of runstop (the button will return to non-blinking). This service acts as a programmatic way to achieve the same effect. When this service is triggered, the [mode topic](#mode-stdmsgsstringhttpsdocsrosorgennoeticapistdmsgshtmlmsgstringhtml) will reflect that the robot is in "runstopped" mode, and after the robot is taken out of runstop, the driver will switch back to whatever mode the robot was in before this service was triggered. While stretch_driver is in "runstopped" mode, no commands to the [cmd_vel topic](#TODO) or the [follow joint trajectory action service](#TODO) will be accepted.

stretch_demos/launch/grasp_object.launch

@@ -1,6 +1,8 @@
 <launch>
 
   <arg name="debug_directory" value="$(env HELLO_FLEET_PATH)/debug/"/>
+  <arg name="dryrun" value="false" />
+  <arg name="rviz" value="true" />
 
   <!-- REALSENSE D435i -->
   <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
@@ -37,6 +39,7 @@
   <!-- GRASP OBJECT -->
   <node name="grasp_object" pkg="stretch_demos" type="grasp_object" output="screen">
     <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
+    <param name="dryrun" type="bool" value="$(arg dryrun)"/>
   </node>
   <!-- -->
 
@@ -45,7 +48,7 @@
   <!-- -->
 
   <!-- VISUALIZE -->
-  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_demos)/rviz/grasp_object.rviz" />
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_demos)/rviz/grasp_object.rviz" if="$(arg rviz)" />
   <!-- -->
 
 </launch>

stretch_demos/nodes/grasp_object

@@ -87,22 +87,20 @@ class GraspObjectNode(hm.HelloNode):
             at_goal = self.move_base.backward(forward_m, detect_obstacles=False, tolerance_distance_m=tolerance_distance_m)
         
     def trigger_grasp_object_callback(self, request):
-        actually_move = True
         max_lift_m = 1.09
         min_extension_m = 0.01
         max_extension_m = 0.5
 
         # 1. Initial configuration
         use_default_mode = False
-        if use_default_mode: 
+        if use_default_mode:
             # Set the D435i to Default mode for obstacle detection
-            trigger_request = TriggerRequest() 
+            trigger_request = TriggerRequest()
             trigger_result = self.trigger_d435i_default_mode_service(trigger_request)
             rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
-        if actually_move:
-            rospy.loginfo('Stow the arm.')
-            self.stow_the_robot()
+        rospy.loginfo('Stow the arm.')
+        self.stow_the_robot()
 
         # 2. Scan surface and find grasp target
         self.look_at_surface(scan_time_s = 3.0)
@@ -120,14 +118,13 @@ class GraspObjectNode(hm.HelloNode):
                 success=False,
                 message='lift position unavailable'
             )
-        if actually_move:
-            rospy.loginfo('Raise tool to pregrasp height.')
-            lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
-            lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m)
-            pose = {'joint_lift': lift_to_pregrasp_m}
-            self.move_to_pose(pose)
+        rospy.loginfo('Raise tool to pregrasp height.')
+        lift_to_pregrasp_m = max(self.lift_position + pregrasp_lift_m, 0.1)
+        lift_to_pregrasp_m = min(lift_to_pregrasp_m, max_lift_m)
+        pose = {'joint_lift': lift_to_pregrasp_m}
+        self.move_to_pose(pose)
 
-        if actually_move and self.tool == "tool_stretch_dex_wrist":
+        if self.tool == "tool_stretch_dex_wrist":
             rospy.loginfo('Rotate pitch/roll for grasping.')
             pose = {'joint_wrist_pitch': -0.3, 'joint_wrist_roll': 0.0}
             self.move_to_pose(pose)
@@ -135,88 +132,83 @@ class GraspObjectNode(hm.HelloNode):
         pregrasp_yaw = self.manipulation_view.get_pregrasp_yaw(grasp_target, self.tf2_buffer)
         rospy.loginfo('pregrasp_yaw = {0:.2f} rad'.format(pregrasp_yaw))
         rospy.loginfo('pregrasp_yaw = {0:.2f} deg'.format(pregrasp_yaw * (180.0/np.pi)))
-        if actually_move:
-            rospy.loginfo('Rotate the gripper for grasping.')
-            pose = {'joint_wrist_yaw': pregrasp_yaw}
-            self.move_to_pose(pose)
+        rospy.loginfo('Rotate the gripper for grasping.')
+        pose = {'joint_wrist_yaw': pregrasp_yaw}
+        self.move_to_pose(pose)
 
-            rospy.loginfo('Open the gripper.')
-            pose = {'gripper_aperture': 0.125}
-            self.move_to_pose(pose)
+        rospy.loginfo('Open the gripper.')
+        pose = {'gripper_aperture': 0.125}
+        self.move_to_pose(pose)
 
         pregrasp_mobile_base_m, pregrasp_wrist_extension_m = self.manipulation_view.get_pregrasp_planar_translation(grasp_target, self.tf2_buffer)
         rospy.loginfo('pregrasp_mobile_base_m = {0:.3f} m'.format(pregrasp_mobile_base_m))
         rospy.loginfo('pregrasp_wrist_extension_m = {0:.3f} m'.format(pregrasp_wrist_extension_m))
-        if actually_move:
-            rospy.loginfo('Drive to pregrasp location.')
-            self.drive(pregrasp_mobile_base_m)
-
-            if pregrasp_wrist_extension_m > 0.0:
-                extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m)
-                extension_m = min(extension_m, max_extension_m)
-                rospy.loginfo('Extend tool above surface.')
-                pose = {'wrist_extension': extension_m}
-                self.move_to_pose(pose)
-            else:
-                rospy.loginfo('negative wrist extension for pregrasp, so not extending or retracting.')
-
-        # 4. Grasp the object and lift it
-        grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer)
-        rospy.loginfo('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m))
-        if actually_move:
-            rospy.loginfo('Move the grasp pose from the pregrasp pose.')
+        rospy.loginfo('Drive to pregrasp location.')
+        self.drive(pregrasp_mobile_base_m)
 
-            lift_m = max(self.lift_position + grasp_lift_m, 0.1)
-            lift_m = min(lift_m, max_lift_m)
-
-            extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m)
+        if pregrasp_wrist_extension_m > 0.0:
+            extension_m = max(self.wrist_position + pregrasp_wrist_extension_m, min_extension_m)
             extension_m = min(extension_m, max_extension_m)
-
-            pose = {'translate_mobile_base': grasp_mobile_base_m,
-                    'joint_lift': lift_m,
-                    'wrist_extension': extension_m}
-            self.move_to_pose(pose)
-
-            rospy.loginfo('Attempt to close the gripper on the object.')
-            gripper_aperture_m = grasp_target['width_m'] - 0.18
-            pose = {'gripper_aperture': gripper_aperture_m}
-            self.move_to_pose(pose)
-
-            # Lifting appears to happen before the gripper has
-            # finished unless there is this sleep. Need to look
-            # into this issue.
-            rospy.sleep(3.0)
-
-            rospy.loginfo('Attempt to lift the object.')
-            object_lift_height_m = 0.1
-            lift_m = max(self.lift_position + object_lift_height_m, 0.2)
-            lift_m = min(lift_m, max_lift_m)
-            pose = {'joint_lift': lift_m}
-            self.move_to_pose(pose)
-
-            rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.')
-            pose = {'gripper_aperture': gripper_aperture_m + 0.005}
-            self.move_to_pose(pose)
-
-            rospy.loginfo('Retract the tool.')
-            pose = {'wrist_extension': 0.01}
+            rospy.loginfo('Extend tool above surface.')
+            pose = {'wrist_extension': extension_m}
             self.move_to_pose(pose)
+        else:
+            rospy.loginfo('negative wrist extension for pregrasp, so not extending or retracting.')
 
-            rospy.loginfo('Reorient the wrist.')
-            pose = {'joint_wrist_yaw': 0.0}
-            self.move_to_pose(pose)
+        # 4. Grasp the object and lift it
+        grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m = self.manipulation_view.get_grasp_from_pregrasp(grasp_target, self.tf2_buffer)
+        rospy.loginfo('grasp_mobile_base_m = {0:3f} m, grasp_lift_m = {1:3f} m, grasp_wrist_extension_m = {2:3f} m'.format(grasp_mobile_base_m, grasp_lift_m, grasp_wrist_extension_m))
+        rospy.loginfo('Move the grasp pose from the pregrasp pose.')
+
+        lift_m = max(self.lift_position + grasp_lift_m, 0.1)
+        lift_m = min(lift_m, max_lift_m)
+        extension_m = max(self.wrist_position + grasp_wrist_extension_m, min_extension_m)
+        extension_m = min(extension_m, max_extension_m)
+        pose = {'translate_mobile_base': grasp_mobile_base_m,
+                'joint_lift': lift_m,
+                'wrist_extension': extension_m}
+        self.move_to_pose(pose)
+
+        rospy.loginfo('Attempt to close the gripper on the object.')
+        gripper_aperture_m = grasp_target['width_m'] - 0.18
+        pose = {'gripper_aperture': gripper_aperture_m}
+        self.move_to_pose(pose)
+
+        # Lifting appears to happen before the gripper has
+        # finished unless there is this sleep. Need to look
+        # into this issue.
+        rospy.sleep(3.0)
+
+        rospy.loginfo('Attempt to lift the object.')
+        object_lift_height_m = 0.1
+        lift_m = max(self.lift_position + object_lift_height_m, 0.2)
+        lift_m = min(lift_m, max_lift_m)
+        pose = {'joint_lift': lift_m}
+        self.move_to_pose(pose)
+
+        rospy.loginfo('Open the gripper a little to avoid overtorquing and overheating the gripper motor.')
+        pose = {'gripper_aperture': gripper_aperture_m + 0.005}
+        self.move_to_pose(pose)
+
+        rospy.loginfo('Retract the tool.')
+        pose = {'wrist_extension': 0.01}
+        self.move_to_pose(pose)
+
+        rospy.loginfo('Reorient the wrist.')
+        pose = {'joint_wrist_yaw': 0.0}
+        self.move_to_pose(pose)
 
         return TriggerResponse(
             success=True,
             message='Completed object grasp!'
-            )
+        )
 
     def main(self):
         hm.HelloNode.main(self, 'grasp_object', 'grasp_object', 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.dryrun = rospy.get_param('~dryrun', False)
 
         self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)