Updates to ML Objectives and MTC Parameters

src/picknik_ur_site_config/objectives/close_cabinet_door.xml

@@ -9,8 +9,15 @@
             <Control ID="Sequence">
                 <Action ID="InitializeMTCTask" task_id="close_cabinet_door" controller_names="/joint_trajectory_controller" task="{close_cabinet_door_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{close_cabinet_door_task}"/>
-                <Action ID="GetPoseFromUser" parameter_name="close_cabinet_door.target_pose" parameter_value="{target_pose}" />
-                <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
+                <Action ID="GetPointsFromUser" view_name="/wrist_mounted_camera/color/image_raw" point_names="target_pose" point_prompts="Click on the door you want to close" pixel_coords="{pixel_coords}"/>
+                <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+                <Action ID="GetPoseFromPixelCoords" point_cloud="{point_cloud}" pixel_coords="{pixel_coords}" downsample_fraction="0.1" neighbor_radius="0.1" output_poses="{output_poses}"/>
+                <Decorator ID="ForEachPoseStamped" vector_in="{output_poses}" out="{pose}">
+                    <Control ID="Sequence">
+                        <Action ID="TransformPose" output_pose="{target_pose}" quaternion_xyzw="0.7071;0.7071;0;0" translation_xyz="0;0;0.1" input_pose="{pose}"/>
+                        <Action ID="SetupMTCMoveToPose" ik_frame="grasp_link" planning_group_name="manipulator" target_pose="{target_pose}" task="{close_cabinet_door_task}" use_all_planners="false"/>
+                    </Control>
+                </Decorator>
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="AllowGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />
                 <Action ID="SetupMTCMoveAlongFrameAxis" task="{close_cabinet_door_task}" hand_frame="grasp_link" axis_frame="grasp_link" axis_x="0.0" axis_y="0.0" axis_z="1.0" max_distance="1.5" min_distance="0.05" planning_group_name="manipulator" velocity_scale="0.05" acceleration_scale="0.05" />                
                 <Action ID="SetupMTCUpdateGroupCollisionRule" name="ForbidGripperCollisionWithOctomap" parameters="{parameters}" task="{close_cabinet_door_task}" />

src/picknik_ur_site_config/objectives/detect_door_graspable_object.xml

@@ -1,7 +1,7 @@
-<?xml version="1.0" encoding="UTF-8"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Detect Door Graspable Object">
   <!-- ////////// -->
-  <BehaviorTree ID="Detect Door Graspable Object" _description="Detects a door grapsable object from a depth image and gets a GraspableObject.">
+  <BehaviorTree ID="Detect Door Graspable Object" _description="Segments out a door from an input depth image and attempts to fit an output GraspableObject.">
     <!-- TODO: need to be directly in front of door for this objective to work until GetGraspableObjectsFromMasks3D calculates the collision more robustly for non cuboid objects. -->
     <Control ID="Sequence" name="TopLevelSequence">
       <!-- Get the point cloud from the wrist camera -->
@@ -11,24 +11,23 @@
       <Action ID="SendPointCloudToUI" point_cloud="{point_cloud}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
       <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
       <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
-      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.05" max_relative_area="0.8" timeout_sec="10.0" masks2d="{masks2d}" valid_classes="{detection_classes}"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0.05" max_relative_area="0.8" timeout_sec="15.0" masks2d="{masks2d}" valid_classes="{detection_classes}"/>
       <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
         <Control ID="Sequence">
           <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
           <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
         </Control>
       </Decorator>
-      <!-- Convert 3D mask into graspable object -->
-      <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{door_objects}"/>
+      <!-- Convert 3D mask into graspable object, but as we don't need it for MTC  -->
+      <Decorator ID="ForceSuccess">
+        <Action ID="GetGraspableObjectsFromMasks3D" point_cloud="{point_cloud}" masks3d="{masks3d}" base_frame="world" plane_inlier_threshold="0.01" minimum_face_area="0.000625" face_separation_threshold="0.02" graspable_objects="{door_objects}"/>
+      </Decorator>
       <Decorator ID="ForceSuccess">
         <Decorator ID="ForEachGraspableObject" vector_in="{door_objects}" out="{door_object}">
           <Action ID="AlwaysFailure"/>
         </Decorator>
       </Decorator>
-      <!-- Add object to planning scene and return success  -->
-      <Action ID="ModifyObjectInPlanningScene" object="{door_object}" apply_planning_scene_service="/apply_planning_scene"/>
-      <Action ID="LogMessage" message="Found a GraspableObject for the door." log_level="info"/>
     </Control>
   </BehaviorTree>
 </root>

src/picknik_ur_site_config/objectives/detect_fixed_handle_graspable_object.xml

@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8"?>
+<?xml version='1.0' encoding='UTF-8'?>
 <root BTCPP_format="4" main_tree_to_execute="Detect Fixed Handle Graspable Object">
   <!-- ////////// -->
   <BehaviorTree ID="Detect Fixed Handle Graspable Object" _description="Detects a fixed-handle graspable object from a depth image and returns a PoseStamped for the handle centroid.">
@@ -12,7 +12,7 @@
         <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
       </Control>
       <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
-      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="10.0" masks2d="{handle_masks2d}" valid_classes="pull handle"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="15.0" masks2d="{handle_masks2d}" valid_classes="pull handle"/>
       <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
       <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
         <Control ID="Sequence">
@@ -27,10 +27,9 @@
           <Action ID="AlwaysFailure"/>
         </Decorator>
       </Decorator>
-      <!-- Add object to planning scene and return success  -->
-      <Action ID="ModifyObjectInPlanningScene" object="{handle_object}" apply_planning_scene_service="/apply_planning_scene"/>
       <Action ID="ExtractGraspableObjectPose" graspable_object="{handle_object}" pose="{grasp_pose}"/>
       <Action ID="LogMessage" message="Successfully extracted necessary subframes for the detected pull handle." log_level="info"/>
+      <Action ID="ModifyObjectInPlanningScene" object="{handle_object}" apply_planning_scene_service="/apply_planning_scene"/>
     </Control>
   </BehaviorTree>
 </root>

src/picknik_ur_site_config/objectives/detect_hinge_line_segment.xml

@@ -17,7 +17,7 @@
         </Decorator>
       </Decorator>
       <!-- fit 3D mask to a line segment and return the two end poses. -->
-      <Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.02" line_segment_start_point="{line_segment_start_point}" line_segment_end_point="{line_segment_end_point}"/>
+      <Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.02" line_segment_poses="{line_segment_poses}"/>
       <Action ID="LogMessage" message="Fit a line segment to the detected hinge successfully." log_level="info"/>
     </Control>
   </BehaviorTree>

src/picknik_ur_site_config/objectives/detect_lever_handle_line_segment.xml

@@ -0,0 +1,27 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Detect Lever Handle Line Segment">
+  <!-- ////////// -->
+  <BehaviorTree ID="Detect Lever Handle Line Segment" _description="Detects a lever-handle Line Segment from a depth image and returns 2 PoseStampeds to represent the handle.">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <!-- Get the point cloud from the wrist camera -->
+      <Control ID="Sequence" name="GetDepthImage">
+        <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
+        <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
+        <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+        <Action ID="UpdatePlanningSceneService" point_cloud="{point_cloud}" point_cloud_service="/point_cloud_service"/>
+      </Control>
+      <!-- Segment out 2D masks and then convert to 3D masks and visualize in UI -->
+      <Action ID="GetMasks2DAction" image="{image}" action_name="get_masks_2d_maskrcnn" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="10.0" masks2d="{handle_masks2d}" valid_classes="handle"/>
+      <Action ID="GetMasks3DFromMasks2D" point_cloud="{point_cloud}" masks2d="{handle_masks2d}" camera_info="{camera_info}" masks3d="{masks3d}"/>
+      <Decorator ID="ForEachMask3D" vector_in="{masks3d}" out="{mask3d}">
+        <Control ID="Sequence">
+          <Action ID="GetPointCloudFromMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" point_cloud_fragment="{point_cloud_fragment}"/>
+          <Action ID="SendPointCloudToUI" point_cloud="{point_cloud_fragment}" sensor_name="scene_scan_camera" point_cloud_uuid="" pcd_topic="/pcd_pointcloud_captures"/>
+        </Control>
+      </Decorator>
+      <!-- Fit a line segment to the 3D mask -->
+      <Action ID="FitLineSegmentToMask3D" point_cloud="{point_cloud}" mask3d="{mask3d}" base_frame="world" max_distance_from_line_for_inlier="0.01" line_segment_poses="{handle_poses}"/>
+      <Action ID="LogMessage" message="Successfully extracted necessary subframes for the detected lever handle." log_level="info"/>
+    </Control>
+  </BehaviorTree>
+</root>