Merge pull request #261 from PickNikRobotics/merge-v5.0-into-main

Merge v5.0 into main

src/picknik_ur_base_config/config/config.yaml

@@ -196,6 +196,7 @@ ros2_control:
     - "joint_trajectory_controller"
     - "admittance_controller_open_door"
     - "joint_trajectory_controller_chained_open_door"
+    - "joint_trajectory_admittance_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []

src/picknik_ur_base_config/config/control/picknik_ur.ros2_control.yaml

@@ -19,6 +19,8 @@ controller_manager:
       type: admittance_controller/AdmittanceController
     joint_trajectory_controller_chained_open_door:
       type: joint_trajectory_controller/JointTrajectoryController
+    joint_trajectory_admittance_controller:
+      type: moveit_pro_controllers/JointTrajectoryAdmittanceController
 
 io_and_status_controller:
   ros__parameters:
@@ -258,3 +260,18 @@ admittance_controller_open_door:
 
     # general settings
     enable_parameter_update_without_reactivation: true
+
+joint_trajectory_admittance_controller:
+  ros__parameters:
+    joints:
+      - shoulder_pan_joint
+      - shoulder_lift_joint
+      - elbow_joint
+      - wrist_1_joint
+      - wrist_2_joint
+      - wrist_3_joint
+    base_frame: base_link
+    tip_frame: grasp_link
+    sensor_frame: tool0
+    ee_frame: grasp_link
+    ft_sensor_name: tcp_fts_sensor

src/picknik_ur_base_config/package.xml

@@ -14,6 +14,7 @@
   <exec_depend>admittance_controller</exec_depend>
   <exec_depend>kinematics_interface_kdl</exec_depend>
   <exec_depend>moveit_planners_stomp</exec_depend>
+  <exec_depend>moveit_pro_controllers</exec_depend>
   <exec_depend>moveit_ros_perception</exec_depend>
   <exec_depend>moveit_studio_agent</exec_depend>
   <exec_depend>moveit_studio_behavior</exec_depend>

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>