Merge 4.0.0 to main

Dockerfile

@@ -61,12 +61,6 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
       --from-paths src \
       --ignore-src
 
-# Remove duplicated serial dependency install files, otherwise there will be a build error.
-# NOTE: This can be removed when MoveIt Pro fully separates core and hardware capabilities.
-WORKDIR /opt/overlay_ws
-RUN rm -rf build/serial install/serial
-
-
 ###################################################################
 # Target for the developer build which does not compile any code. #
 ###################################################################

src/picknik_ur_base_config/objectives/3_waypoint_pick_and_place.xml

@@ -5,28 +5,28 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
                 <Control ID="Sequence">
                     <!-- Pick object from "Grab", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Grab", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick Block" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                 </Control>
             </Decorator>

src/picknik_ur_base_config/objectives/cycle_between_waypoints.xml

@@ -3,8 +3,8 @@
     <BehaviorTree ID="Cycle Between Waypoints" _description="Cycles between two waypoints until failure">
         <Decorator ID="KeepRunningUntilFailure">
             <Control ID="Sequence">
-                <Action ID="MoveToWaypoint" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Right Corner" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             </Control>
         </Decorator>
     </BehaviorTree>

src/picknik_ur_base_config/objectives/move_to_joint_state.xml

@@ -6,7 +6,7 @@
                 <Action ID="RetrieveJointStateParameter" timeout_sec="" joint_state="{target_joint_state}"/>
                 <Action ID="InitializeMTCTask" task_id="move_to_joint_state" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" task="{move_to_waypoint_task}"/>
                 <Action ID="SetupMTCCurrentState" task="{move_to_waypoint_task}"/>
-                <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" use_all_planners="true" constraints=""/>
+                <Action ID="SetupMTCMoveToJointState" joint_state="{target_joint_state}" name="SetupMTCMoveToJointState_First" planning_group_name="manipulator" task="{move_to_waypoint_task}" use_all_planners="false" constraints=""/>
                 <Action ID="PlanMTCTask" solution="{move_to_waypoint_solution}" task="{move_to_waypoint_task}"/>
                 <Action ID="ExecuteMTCTask" solution="{move_to_waypoint_solution}"/>
                 <Action ID="PublishEmpty" topic="/studio_ui/motion_ended" queue_size="1" use_best_effort="false"/>

src/picknik_ur_base_config/objectives/teleoperate.xml

@@ -2,6 +2,6 @@
 <root BTCPP_format="4" main_tree_to_execute="Teleoperate">
   <!-- ////////// -->
   <BehaviorTree ID="Teleoperate" _description="Handles the different variations of teleoperation from the web UI. Can be used standalone." _favorite="true" _hardcoded="false">
-    <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="0"/>
+    <SubTree ID="Request Teleoperation" enable_user_interaction="false" user_interaction_prompt="" initial_teleop_mode="3"/>
   </BehaviorTree>
 </root>

src/picknik_ur_gazebo_config/objectives/3_waypoint_pick_and_place.xml

@@ -5,28 +5,28 @@
         <Control ID="Sequence">
             <!-- Clear snapshot, move to center pose, and open gripper -->
             <Action ID="ClearSnapshot" />
-            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+            <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
             <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
             <!-- Keep executing the pick and place sequence until failure -->
             <Decorator ID="KeepRunningUntilFailure">
                 <Control ID="Sequence">
                     <!-- Pick object from "Pick", put it down at "Place", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                     <!-- Pick object from "Place", put it down at "Pick", and go back to center pose -->
                     <Control ID="Sequence">
-                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Place" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0.7929"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Pick" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                         <Action ID="MoveGripperAction" gripper_command_action_name="/robotiq_gripper_controller/gripper_cmd" position="0"/>
-                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="true"/>
+                        <Action ID="MoveToWaypoint" waypoint_name="Look at Table" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
                     </Control>
                 </Control>
             </Decorator>

src/picknik_ur_gazebo_config/objectives/object_segmentation_2d.xml

@@ -0,0 +1,11 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Object Segmentation 2D">
+  <!-- ////////// -->
+  <BehaviorTree ID="Object Segmentation 2D" _description="Detect objects using a 2D image segmentation model." _favorite="false" _hardcoded="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
+      <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
+      <Action ID="GetMasks2DAction" image="{image}" action_name="/get_masks_2d_maskrcnn" valid_classes="" min_confidence="0.8" max_nms_iou="0.8" min_relative_area="0" max_relative_area="1" timeout_sec="-1" masks2d="{masks2d}"/>
+    </Control>
+  </BehaviorTree>
+</root>

src/picknik_ur_gazebo_config/objectives/object_segmentation_3d.xml

@@ -0,0 +1,26 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<root BTCPP_format="4" main_tree_to_execute="Object Segmentation 3D">
+  <!-- ////////// -->
+  <BehaviorTree ID="Object Segmentation 3D" _description="Detect objects using a 2D image segmentation model and convert them to 3D masks and graspable objects." _favorite="false" _hardcoded="false">
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="ClearSnapshot"/>
+      <Action ID="ResetPlanningSceneObjects" apply_planning_scene_service="/apply_planning_scene"/>
+      <Action ID="MoveToWaypoint" waypoint_name="Extended Right" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false" constraints=""/>
+      <Action ID="GetCameraInfo" topic_name="/wrist_mounted_camera/color/camera_info" message_out="{camera_info}"/>
+      <Action ID="GetPointCloud" topic_name="/wrist_mounted_camera/depth/color/points" message_out="{point_cloud}"/>
+      <Action ID="GetImage" topic_name="/wrist_mounted_camera/color/image_raw" message_out="{image}"/>
+      <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="0.8" timeout_sec="-1" masks2d="{masks2d}" valid_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>
+      <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.01" graspable_objects="{graspable_objects}"/>
+      <Decorator ID="ForEachGraspableObject" vector_in="{graspable_objects}" out="{object}">
+        <Action ID="ModifyObjectInPlanningScene" object="{object}" apply_planning_scene_service="/apply_planning_scene"/>
+      </Decorator>
+    </Control>
+  </BehaviorTree>
+</root>

src/picknik_ur_gazebo_config/waypoints/waypoints.yaml

@@ -349,7 +349,7 @@
     velocity: []
   name: Monitor View
 - description: ''
-  favorite: true
+  favorite: false
   joint_state:
     effort: []
     header: