Merge 8.8 into main

Dockerfile

@@ -67,7 +67,7 @@ RUN groupadd realtime && \
 # hadolint ignore=SC1091
 RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
     --mount=type=cache,target=/var/lib/apt,sharing=locked \
-    --mount=type=bind,target=${USER_WS}/,source=. \
+    --mount=type=bind,target=${USER_WS}/src,source=./src \
     . /opt/overlay_ws/install/setup.sh && \
     apt-get update && \
     rosdep install -q -y \
@@ -105,139 +105,11 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
         less \
         gdb \
         nano \
-	tmux
+        tmux
 
 # Set up the user's .bashrc file and shell.
 CMD ["/usr/bin/bash"]
 
-
-##################################################
-# Starting from the specified MoveIt Pro release with CUDA GPU #
-##################################################
-# The image tag is specified in the argument itself.
-# hadolint ignore=DL3006
-FROM ${MOVEIT_STUDIO_BASE_IMAGE} AS base-gpu
-
-# Create a non-root user
-ARG USERNAME
-ARG USER_UID
-ARG USER_GID
-
-# hadolint ignore=DL3008
-RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
-    --mount=type=cache,target=/var/lib/apt,sharing=locked \
-    apt-get update && apt-get install wget -y -q --no-install-recommends && \
-    wget --progress=dot:giga https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2204/x86_64/cuda-keyring_1.1-1_all.deb && \
-    dpkg -i cuda-keyring_1.1-1_all.deb && \
-    apt-get update && \
-    apt-get install -q -y --no-install-recommends \
-      libcudnn9-cuda-12 \
-      libcudnn9-dev-cuda-12 \
-      libcublas-12-6 \
-      cuda-cudart-12-6 \
-      libcurand-12-6 \
-      libcufft-12-6 \
-      libnvinfer10 \
-      libnvinfer-plugin10 \
-      libnvonnxparsers10 \
-      libtree
-
-# Misleading name: onnxruntime_gpu is actually specifically the CUDA package. This is only shipped for x86-64
-RUN if [ "$(uname -m)" = "x86_64" ]; then pip3 install --no-cache-dir onnxruntime_gpu==1.19.0; fi
-
-# Copy source code from the workspace's ROS 2 packages to a workspace inside the container
-ARG USER_WS=/home/${USERNAME}/user_ws
-ENV USER_WS=${USER_WS}
-
-# Also mkdir with user permission directories which will be mounted later to avoid docker creating them as root
-WORKDIR $USER_WS
-# hadolint ignore=DL3008
-RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
-    --mount=type=cache,target=/var/lib/apt,sharing=locked \
-    groupadd --gid $USER_GID ${USERNAME} && \
-    useradd --uid $USER_UID --gid $USER_GID --shell /bin/bash --create-home ${USERNAME} && \
-    apt-get update && \
-    apt-get install -q -y --no-install-recommends sudo && \
-    echo ${USERNAME} ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/${USERNAME} && \
-    chmod 0440 /etc/sudoers.d/${USERNAME} && \
-    cp -r /etc/skel/. /home/${USERNAME} && \
-    mkdir -p \
-      /home/${USERNAME}/.ccache \
-      /home/${USERNAME}/.config \
-      /home/${USERNAME}/.ignition \
-      /home/${USERNAME}/.colcon \
-      /home/${USERNAME}/.ros && \
-    chown -R $USER_UID:$USER_GID /home/${USERNAME} /opt/overlay_ws/
-
-# Install additional dependencies
-# You can also add any necessary apt-get install, pip install, etc. commands at this point.
-# NOTE: The /opt/overlay_ws folder contains MoveIt Pro binary packages and the source file.
-# hadolint ignore=SC1091
-RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
-    --mount=type=cache,target=/var/lib/apt,sharing=locked \
-    --mount=type=bind,target=${USER_WS}/,source=. \
-    . /opt/overlay_ws/install/setup.sh && \
-    apt-get update && \
-    rosdep install -q -y \
-      --from-paths src \
-      --ignore-src
-
-# Set up colcon defaults for the new user
-USER ${USERNAME}
-RUN colcon mixin add default \
-    https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \
-    colcon mixin update && \
-    colcon metadata add default \
-    https://raw.githubusercontent.com/colcon/colcon-metadata-repository/master/index.yaml && \
-    colcon metadata update
-COPY colcon-defaults.yaml /home/${USERNAME}/.colcon/defaults.yaml
-
-# hadolint ignore=DL3002
-USER root
-
-# Set up the user's .bashrc file and shell.
-CMD ["/usr/bin/bash"]
-
-###################################################################
-# Target for the developer build which does not compile any code. #
-###################################################################
-FROM base-gpu  AS user-overlay-gpu-dev
-
-ARG USERNAME
-ARG USER_WS=/home/${USERNAME}/user_ws
-ENV USER_WS=${USER_WS}
-
-# Install any additional packages for development work
-# hadolint ignore=DL3008
-RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
-    --mount=type=cache,target=/var/lib/apt,sharing=locked \
-    apt-get update && \
-    apt-get install -y --no-install-recommends \
-        less \
-        gdb \
-        nano
-
-# Set up the user's .bashrc file and shell.
-CMD ["/usr/bin/bash"]
-
-#########################################
-# Target for compiled, deployable image with GPU support #
-#########################################
-FROM base-gpu AS user-overlay-gpu
-
-ARG USERNAME
-ARG USER_WS=/home/${USERNAME}/user_ws
-ENV USER_WS=${USER_WS}
-
-ENV LD_LIBRARY_PATH=/usr/local/lib/python3.10/dist-packages/onnxruntime/capi:/usr/lib/x86_64-linux-gnu:/usr/local/cuda-12.6/targets/x86_64-linux/lib:$LD_LIBRARY_PATH
-
-# Compile the workspace
-WORKDIR $USER_WS
-
-# Set up the user's .bashrc file and shell.
-CMD ["/usr/bin/bash"]
-
-
 #########################################
 # Target for compiled, deployable image #
 #########################################

src/factory_sim/objectives/reachability_analysis_-_blocks.xml

@@ -7,6 +7,11 @@
     _favorite="true"
   >
     <Control ID="Sequence" name="">
+      <Action
+        ID="LogMessage"
+        log_level="info"
+        message="Some of the poses in this objective are unreachable, and planning will fail. A warning with instructions how to continue will appear here"
+      />
       <Control ID="Sequence" name="Add visualization from scene camera">
         <SubTree ID="Clear Snapshot" _collapsed="true" />
         <SubTree ID="Take Snapshot" _collapsed="true" />

src/lab_sim/config/config.yaml

@@ -32,6 +32,9 @@ moveit_params:
   pose_jog:
     package: "lab_sim"
     path: "config/moveit/pose_jog.yaml"
+  sensors_3d:
+    package: "lab_sim"
+    path: "config/moveit/sensors_3d.yaml"
 
 # Configuration for loading behaviors and objectives.
 # [Required]

src/lab_sim/config/moveit/sensors_3d.yaml

@@ -7,18 +7,18 @@ scene_scan_camera:
   # Set to an empty topic to disable.
   point_cloud_service_name: "/point_cloud_service"
   # Points further than this will not be used (in meters).
-  max_range: 1.5
+  max_range: 6.0
   # Choose one of every 'point_subsample' points (select all if set to 1).
   point_subsample: 1
   # Should always be >= 1.0. Scale up collision shapes in the scene before excluding them from the octomap.
   padding_scale: 1.0
   # Absolute padding around scaled collision shapes when excluding them from the octomap (in meters).
-  padding_offset: 0.01
+  padding_offset: 0.1
   # The octomap representation will be updated at rate less than or equal to this value.
   max_update_rate: 0.1
 
 # Specifies the resolution at which the octomap is maintained (in meters).
-octomap_resolution: 0.01
+octomap_resolution: 0.05
 # Specifies the coordinate frame in which the Octomap representation will be stored.
 # Note! When an OccupancyMonitor instance is initialized by the PlanningSceneMonitor,
 # this frame parameter will not be used. Instead, the frame defaults to the planning frame.

src/lab_sim/objectives/_scan_scene.xml

@@ -14,6 +14,13 @@
         joint_group_name="manipulator"
         controller_names="joint_trajectory_controller"
         waypoint_name="Park Far Right"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        seed="0"
+        velocity_scale_factor="1.0"
       />
       <SubTree ID="Take Scene Camera Snapshot" _collapsed="true" />
       <SubTree
@@ -22,6 +29,13 @@
         joint_group_name="manipulator"
         controller_names="joint_trajectory_controller"
         waypoint_name="Look at Table"
+        acceleration_scale_factor="1.0"
+        controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+        keep_orientation="false"
+        keep_orientation_tolerance="0.05"
+        link_padding="0.01"
+        seed="0"
+        velocity_scale_factor="1.0"
       />
     </Control>
   </BehaviorTree>

src/lab_sim/objectives/close_gripper.xml

@@ -1,8 +1,9 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Close Gripper">
   <BehaviorTree
     ID="Close Gripper"
     _description="Close the gripper"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="root">
       <Action

src/lab_sim/objectives/grasp_pose_using_yaml.xml

@@ -4,7 +4,7 @@
   <BehaviorTree
     ID="Grasp Pose Using Yaml"
     _description="Reads a YAML file for grasp pose offset and picks up an object that has an AprilTag marker."
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <SubTree ID="Open Gripper" />

src/lab_sim/objectives/joint_diagnostic.xml

@@ -1,9 +1,10 @@
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Joint Diagnostic">
   <!--//////////-->
   <BehaviorTree
     ID="Joint Diagnostic"
     _description="Example of cycling through the min and max limits of a single joint, for hardware testing"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Setup by moving home-->

src/lab_sim/objectives/load_and_execute_joint_trajectory.xml

@@ -1,10 +1,10 @@
-<?xml version="1.0" encoding="utf-8" ?>
+<?xml version="1.0" encoding="UTF-8" ?>
 <root BTCPP_format="4" main_tree_to_execute="Load and Execute Joint Trajectory">
   <!--//////////-->
   <BehaviorTree
     ID="Load and Execute Joint Trajectory"
     _description="Load a trajectory that was saved as a YAML file"
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence" name="TopLevelSequence">
       <!--Make sure to move the robot to the same starting position as the saved trajectory starts, otherwise the joint_trajectory_controller will use joint interpolation to align the robot with the path-->

src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml

@@ -29,7 +29,6 @@
   <TreeNodesModel>
     <SubTree ID="Load Mesh as Red Point Cloud">
       <output_port name="point_cloud" default="{red_cloud}" />
-      <input_port name="_collapsed" default="false" />
       <input_port name="initial_pose" default="{stamped_pose}" />
       <MetadataFields>
         <Metadata subcategory="Perception - 3D Point Cloud" />

src/lab_sim/objectives/ml_segment_image_from_text_prompt.xml → src/lab_sim/objectives/ml_segment_image.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="ML Segment Image from Text Prompt">
+<root BTCPP_format="4" main_tree_to_execute="ML Segment Image">
   <!--//////////-->
   <BehaviorTree
-    ID="ML Segment Image from Text Prompt"
-    _description="Run an ML model to find all objects and visualize the masks with bounding boxes."
+    ID="ML Segment Image"
+    _description="Run an ML model to find all objects and visualize the masks with bounding boxes, from a text prompt."
     _favorite="true"
   >
     <Control ID="Sequence">
@@ -27,7 +27,7 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="ML Segment Image from Text Prompt">
+    <SubTree ID="ML Segment Image">
       <MetadataFields>
         <Metadata runnable="true" />
         <Metadata subcategory="Application - ML (GPU Recommended)" />

src/lab_sim/objectives/ml_segment_point_cloud_from_text_prompt.xml → src/lab_sim/objectives/ml_segment_point_cloud.xml

@@ -1,12 +1,9 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root
-  BTCPP_format="4"
-  main_tree_to_execute="ML Segment Point Cloud from Text Prompt"
->
+<root BTCPP_format="4" main_tree_to_execute="ML Segment Point Cloud">
   <!--//////////-->
   <BehaviorTree
-    ID="ML Segment Point Cloud from Text Prompt"
-    _description="Captures a point cloud and segments out all possible objects."
+    ID="ML Segment Point Cloud"
+    _description="Captures a point cloud and segments out all possible objects from a text prompt"
     _favorite="true"
   >
     <Control ID="Sequence">
@@ -32,7 +29,7 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="ML Segment Point Cloud from Text Prompt">
+    <SubTree ID="ML Segment Point Cloud">
       <MetadataFields>
         <Metadata runnable="true" />
         <Metadata subcategory="Application - ML (GPU Recommended)" />

src/lab_sim/objectives/ml_segment_point_cloud_from_clicked_point.xml

@@ -7,7 +7,7 @@
   <BehaviorTree
     ID="ML Segment Point Cloud from Clicked Point"
     _description="Captures a point cloud and requests the user to click on three objects in the image to be segmented. The point cloud is then filtered to only include the selected objects."
-    _favorite="true"
+    _favorite="false"
   >
     <Control ID="Sequence">
       <SubTree

src/lab_sim/objectives/octomap_example.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<root BTCPP_format="4" main_tree_to_execute="Octomap Example">
+  <!--//////////-->
+  <BehaviorTree
+    ID="Octomap Example"
+    _description="Takes a snapshot of the scene camera and then updates the planning scene in RViz with an ocotomap based on the `sensors_3d.yaml` file settings."
+    _favorite="false"
+  >
+    <Control ID="Sequence" name="TopLevelSequence">
+      <Action ID="ClearSnapshot" />
+      <Action
+        ID="GetPointCloud"
+        message_out="{point_cloud}"
+        publisher_timeout_sec="5.000000"
+        timeout_sec="5.000000"
+        topic_name="/scene_camera/points"
+      />
+      <Action
+        ID="UpdatePlanningSceneService"
+        point_cloud="{point_cloud}"
+        point_cloud_service="/point_cloud_service"
+      />
+      <Action
+        ID="SendPointCloudToUI"
+        pcd_topic="/pcd_pointcloud_captures"
+        point_cloud="{point_cloud}"
+      />
+    </Control>
+  </BehaviorTree>
+  <TreeNodesModel>
+    <SubTree ID="Octomap Example">
+      <MetadataFields>
+        <Metadata runnable="true" />
+        <Metadata subcategory="Perception - 3D Point Cloud" />
+      </MetadataFields>
+    </SubTree>
+  </TreeNodesModel>
+</root>

src/lab_sim/objectives/ml_grasp_object_from_text_prompt.xml → src/lab_sim/objectives/pick_1_pill_bottle.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0" encoding="UTF-8" ?>
-<root BTCPP_format="4" main_tree_to_execute="ML Grasp Object from Text Prompt">
+<root BTCPP_format="4" main_tree_to_execute="Pick 1 Pill Bottle">
   <!--//////////-->
   <BehaviorTree
-    ID="ML Grasp Object from Text Prompt"
+    ID="Pick 1 Pill Bottle"
     _description="Captures and segments objects from a point cloud based on a text prompt. Then grasps the first object returned from the segmentation."
     _favorite="true"
   >
@@ -132,7 +132,7 @@
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
-    <SubTree ID="ML Grasp Object from Text Prompt">
+    <SubTree ID="Pick 1 Pill Bottle">
       <MetadataFields>
         <Metadata runnable="true" />
         <Metadata subcategory="Application - ML (GPU Recommended)" />