Merge pull request #6 from hello-robot/develop

Merge develop into master

hello_helpers/src/hello_helpers/hello_misc.py

@@ -226,14 +226,20 @@ def get_p1_to_p2_matrix(p1_frame_id, p2_frame_id, tf2_buffer, lookup_time=None,
         print('         exception =', e)
         return None, None
 
-def bound_ros_command(bounds, ros_pos, clip_ros_tolerance=1e-3):
+def bound_ros_command(bounds, ros_pos, fail_out_of_range_goal, clip_ros_tolerance=1e-3):
     """Clip the command with clip_ros_tolerance, instead of
     invalidating it, if it is close enough to the valid ranges.
     """
     if ros_pos < bounds[0]:
-        return bounds[0] if (bounds[0] - ros_pos) < clip_ros_tolerance else None
+        if fail_out_of_range_goal:
+            return bounds[0] if (bounds[0] - ros_pos) < clip_ros_tolerance else None
+        else:
+            return bounds[0]
 
     if ros_pos > bounds[1]:
-        return bounds[1] if (ros_pos - bounds[1]) < clip_ros_tolerance else None
+        if fail_out_of_range_goal:
+            return bounds[1] if (ros_pos - bounds[1]) < clip_ros_tolerance else None
+        else:
+            return bounds[1]
 
     return ros_pos

stretch_calibration/launch/collect_head_calibration_data.launch

@@ -13,6 +13,7 @@
 
   <!-- 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"/>
 
   <param name="robot_description"
 	 textfile="$(arg uncalibrated_urdf_file)"/>

stretch_core/launch/keyboard_teleop.launch

@@ -2,6 +2,7 @@
 
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <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"/>
   <!-- -->
     

stretch_core/launch/wheel_odometry_test.launch

@@ -13,6 +13,7 @@
   
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <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"/>
   <!-- -->
   

stretch_core/nodes/command_groups.py

@@ -74,7 +74,7 @@ class SimpleCommandGroup:
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         """Sets goal for the joint group
 
         Sets and validates the goal point for the joints
@@ -86,6 +86,8 @@ class SimpleCommandGroup:
             the target point for all joints
         invalid_goal_callback: func
             error callback for invalid goal
+        fail_out_of_range_goal: bool
+            whether to bound out-of-range goals to range or fail
 
         Returns
         -------
@@ -102,7 +104,7 @@ class SimpleCommandGroup:
                 invalid_goal_callback(err_str)
                 return False
 
-            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos)
+            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
             self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
             self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
             self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
@@ -277,7 +279,7 @@ class GripperCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         if self.active:
             goal_pos = point.positions[self.index] if len(point.positions) > self.index else None
@@ -289,7 +291,7 @@ class GripperCommandGroup(SimpleCommandGroup):
                 return False
 
             joint_range = self.range_aperture_m if (self.name == 'gripper_aperture') else self.range_finger_rad
-            self.goal['position'] = hm.bound_ros_command(joint_range, goal_pos)
+            self.goal['position'] = hm.bound_ros_command(joint_range, goal_pos, fail_out_of_range_goal)
             self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
             self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
             self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
@@ -374,7 +376,7 @@ class TelescopingCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         if self.active:
             if self.is_telescoping:
@@ -403,7 +405,7 @@ class TelescopingCommandGroup(SimpleCommandGroup):
                 invalid_goal_callback(err_str)
                 return False
 
-            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos)
+            self.goal['position'] = hm.bound_ros_command(self.range, goal_pos, fail_out_of_range_goal)
             if self.goal['position'] is None:
                 err_str = ("Received {0} goal point that is out of bounds. "
                             "Range = {1}, but goal point = {2}.").format(self.name, self.range, goal_pos)
@@ -536,7 +538,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def set_goal(self, point, invalid_goal_callback, **kwargs):
+    def set_goal(self, point, invalid_goal_callback, fail_out_of_range_goal, **kwargs):
         self.goal = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         self.goal_translate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
         self.goal_rotate_mobile_base = {"position": None, "velocity": None, "acceleration": None, "contact_threshold": None}
@@ -583,7 +585,7 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
                     invalid_goal_callback(err_str)
                     return False
 
-                self.goal['position'] = self.ros_to_mechaduino(goal_pos, kwargs['manipulation_origin'])
+                self.goal['position'] = self.ros_to_mechaduino(goal_pos, kwargs['manipulation_origin'], fail_out_of_range_goal)
                 self.goal['velocity'] = point.velocities[self.index] if len(point.velocities) > self.index else None
                 self.goal['acceleration'] = point.accelerations[self.index] if len(point.accelerations) > self.index else None
                 self.goal['contact_threshold'] = point.effort[self.index] if len(point.effort) > self.index else None
@@ -595,8 +597,8 @@ class MobileBaseCommandGroup(SimpleCommandGroup):
 
         return True
 
-    def ros_to_mechaduino(self, ros_ros, manipulation_origin):
-        ros_pos = hm.bound_ros_command(self.range, ros_ros)
+    def ros_to_mechaduino(self, ros_ros, manipulation_origin, fail_out_of_range_goal):
+        ros_pos = hm.bound_ros_command(self.range, ros_ros, fail_out_of_range_goal)
         return (manipulation_origin['x'] + ros_pos) if ros_pos is not None else None
 
     def init_execution(self, robot, robot_status, **kwargs):

stretch_core/nodes/joint_trajectory_server.py

@@ -101,7 +101,7 @@ class JointTrajectoryAction:
             rospy.logdebug(("{0} joint_traj action: "
                             "target point #{1} = <{2}>").format(self.node.node_name, pointi, point))
 
-            valid_goals = [c.set_goal(point, self.invalid_goal_callback,
+            valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal,
                                       manipulation_origin=self.node.mobile_base_manipulation_origin)
                            for c in command_groups]
             if not all(valid_goals):

stretch_core/nodes/stretch_driver

@@ -21,6 +21,7 @@ from control_msgs.msg import FollowJointTrajectoryAction
 from control_msgs.msg import FollowJointTrajectoryResult
 
 from std_srvs.srv import Trigger, TriggerResponse
+from std_srvs.srv import SetBool, SetBoolResponse
 
 from nav_msgs.msg import Odometry
 from sensor_msgs.msg import JointState, Imu, MagneticField
@@ -453,6 +454,32 @@ class StretchBodyNode:
             message='Now in position mode.'
             )
 
+    def runstop_service_callback(self, request):
+        if request.data:
+            with self.robot_stop_lock:
+                self.stop_the_robot = True
+
+                self.robot.base.translate_by(0.0)
+                self.robot.base.rotate_by(0.0)
+                self.robot.arm.move_by(0.0)
+                self.robot.lift.move_by(0.0)
+                self.robot.push_command()
+
+                self.robot.head.move_by('head_pan', 0.0)
+                self.robot.head.move_by('head_tilt', 0.0)
+                self.robot.end_of_arm.move_by('wrist_yaw', 0.0)
+                self.robot.end_of_arm.move_by('stretch_gripper', 0.0)
+                self.robot.push_command()
+
+            self.robot.pimu.runstop_event_trigger()
+        else:
+            self.robot.pimu.runstop_event_reset()
+
+        return SetBoolResponse(
+            success=True,
+            message='is_runstopped: {0}'.format(request.data)
+            )
+
     ########### MAIN ############
 
     def main(self):
@@ -553,6 +580,7 @@ class StretchBodyNode:
         self.last_twist_time = rospy.get_time()
 
         # start action server for joint trajectories
+        self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', True)
         self.joint_trajectory_action = JointTrajectoryAction(self)
         self.joint_trajectory_action.server.start()
 
@@ -579,6 +607,10 @@ class StretchBodyNode:
                                                     Trigger,
                                                     self.stop_the_robot_callback)
 
+        self.runstop_service = rospy.Service('/runstop',
+                                              SetBool,
+                                              self.runstop_service_callback)
+
         try:
             # start loop to command the mobile base velocity, publish
             # odometry, and publish joint states

stretch_deep_perception/launch/stretch_detect_body_landmarks.launch

@@ -12,6 +12,7 @@
 
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
   <include file="$(find stretch_core)/launch/stretch_driver.launch" />
   <!-- -->
 

stretch_deep_perception/launch/stretch_detect_faces.launch

@@ -12,6 +12,7 @@
   
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
   <include file="$(find stretch_core)/launch/stretch_driver.launch" />
   <!-- -->
 

stretch_deep_perception/launch/stretch_detect_nearest_mouth.launch

@@ -12,6 +12,7 @@
 
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
   <include file="$(find stretch_core)/launch/stretch_driver.launch" />
   <!-- -->
 

stretch_deep_perception/launch/stretch_detect_objects.launch

@@ -12,6 +12,7 @@
   
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
   <include file="$(find stretch_core)/launch/stretch_driver.launch" />
   <!-- -->
 

stretch_demos/launch/clean_surface.launch

@@ -12,6 +12,7 @@
   
   <!-- 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"/>
   <!-- -->
     

stretch_demos/launch/grasp_object.launch

@@ -12,6 +12,7 @@
   
   <!-- 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"/>
   <!-- -->
 

stretch_demos/launch/handover_object.launch

@@ -11,6 +11,7 @@
   
   <!-- 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"/>
   <!-- -->
 

stretch_demos/launch/hello_world.launch

@@ -12,6 +12,7 @@
   
   <!-- 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"/>
   <!-- -->
 

stretch_demos/launch/open_drawer.launch

@@ -12,6 +12,7 @@
   
   <!-- 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"/>
   <!-- -->
 

stretch_funmap/launch/mapping.launch

@@ -16,6 +16,7 @@
   
   <!-- 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"/>
   <!-- -->
 

stretch_funmap/src/stretch_funmap/segment_max_height_image.py

@@ -29,7 +29,7 @@ def find_object_to_grasp(height_image, display_on=False):
         if display_on: 
             print('No elevated surface found.')
         return None
-
+    
     surface_height_pix = np.max(h_image[surface_mask > 0])
     surface_height_m = m_per_unit * surface_height_pix
     height_image.apply_planar_correction(plane_parameters, surface_height_pix)
@@ -42,11 +42,24 @@ def find_object_to_grasp(height_image, display_on=False):
         rgb_image = height_image.rgb_image.copy()
         rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0] 
 
+    #####################################
+    # Select candidate object points
+
+    # Define the minimum height for a candidate object point
     min_object_height_m = 0.01
     min_obstacle_height_m = surface_height_m + min_object_height_m
     min_obstacle_height_pix = min_obstacle_height_m / m_per_unit
 
-    obstacle_selector = h_image > min_obstacle_height_pix
+    # Define the maximum height for a candidate object point
+    robot_camera_height_m = 1.13 #from HeadScan in mapping.py and ManipulationView in manipulation_planning.py)
+    voi_safety_margin_m = 0.02 
+    max_object_height_m = 0.4
+    max_obstacle_height_m = min(robot_camera_height_m - voi_safety_margin_m,
+                                surface_height_m + max_object_height_m)
+    max_obstacle_height_pix = max_obstacle_height_m / m_per_unit
+
+    # Select candidate object points that are within the valid height range
+    obstacle_selector = (h_image > min_obstacle_height_pix) & (h_image < max_obstacle_height_pix)
 
     if display_on: 
         rgb_image = height_image.rgb_image.copy()
@@ -61,11 +74,18 @@ def find_object_to_grasp(height_image, display_on=False):
         rgb_image[surface_mask > 0] = (rgb_image[surface_mask > 0]/2) + [0, 127, 0] 
         rgb_image[obstacle_mask > 0] = (rgb_image[obstacle_mask > 0]/2) + [0, 0, 127] 
 
+    # Find the convex hull of the surface points to represent the full
+    # surface, overcoming occlusion holes, noise, and other phenomena.
     surface_convex_hull_mask = convex_hull_image(surface_mask)
+
+    # Select candidate object points that are both within the valid
+    # height range and on the surface
     obstacles_on_surface_selector = (obstacle_selector & surface_convex_hull_mask) 
     obstacles_on_surface = np.uint8(255.0 * obstacles_on_surface_selector)
 
-    # Dilate and erode the obstacles to agglomerate object parts. 
+    # Dilate and erode the candidate object points to agglomerate
+    # object parts that might be separated due to occlusion, noise,
+    # and other phenomena.
     kernel_width_pix = 5 #3
     iterations = 3 #5
     kernel_radius_pix = (kernel_width_pix - 1) / 2
@@ -78,6 +98,10 @@ def find_object_to_grasp(height_image, display_on=False):
     if use_erosion:
         obstacles_on_surface = cv2.erode(obstacles_on_surface, kernel, iterations=iterations)
 
+    #####################################
+    # Process the candidate object points      
+        
+    # Treat connected components of candidate object points as objects. Fit ellipses to these segmented objects.
     label_image, max_label_index = sk.measure.label(obstacles_on_surface, neighbors=8, background=0, return_num=True, connectivity=None)
     region_properties = sk.measure.regionprops(label_image, intensity_image=None, cache=True, coordinates='xy')
     if display_on:
@@ -85,7 +109,10 @@ def find_object_to_grasp(height_image, display_on=False):
         color_label_image = sk.color.label2rgb(label_image, image=rgb_image, colors=None, alpha=0.3, bg_label=0, bg_color=(0, 0, 0), image_alpha=1, kind='overlay')
         cv2.imshow('color_label_image', color_label_image)
 
-    if len(region_properties) > 0: 
+    # Proceed if an object was found.
+    if len(region_properties) > 0:
+
+        # Select the object with the largest area.
         largest_region = None
         largest_area = 0.0
         for region in region_properties:
@@ -93,16 +120,21 @@ def find_object_to_grasp(height_image, display_on=False):
                 largest_region = region
                 largest_area = region.area
 
+        # Make the object with the largest area the grasp target. In
+        # the future, other criteria could be used, such as the
+        # likelihood that the gripper can actually grasp the
+        # object. For example, the target object might be too large.
         object_region = largest_region
+
+        # Collect and compute various features for the target object.
         object_ellipse = get_ellipse(object_region)
         object_area_m_sqr = object_region.area * pow(m_per_pix, 2)
         min_row, min_col, max_row, max_col = object_region.bbox
         object_bounding_box = {'min_row': min_row, 'min_col': min_col, 'max_row': max_row, 'max_col': max_col}
 
-        # Only compute height statistics using the
-        # original, high-confidence heights above the
-        # surface that are a part of the final object
-        # region.
+        # Only compute height statistics using the original,
+        # high-confidence heights above the surface that are a part of
+        # the final object region.
         object_selector = (label_image == object_region.label)
         object_height_selector = obstacles_on_surface_selector & object_selector
         object_heights_m = (m_per_unit * h_image[object_height_selector]) - surface_height_m
@@ -123,7 +155,7 @@ def find_object_to_grasp(height_image, display_on=False):
         #            'minor': {'axis': minor_axis, 'length': r.minor_axis_length},
         #            'major': {'axis': major_axis, 'length': r.major_axis_length, 'ang_rad': major_ang_rad}}
 
-        # Create object grasp target.
+        # Prepare grasp target information.
         grasp_location_xy_pix = object_ellipse['centroid']
         major_length_pix = object_ellipse['major']['length']
         major_length_m = m_per_pix * major_length_pix