Update RoboPlan to latest and bring submodules up to date

Dockerfile

@@ -145,6 +145,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
 COPY --chown=${USERNAME}:${USERNAME} src/ src/
 
 # Setup colcon default mixins and add default settings
+RUN touch /home/${USERNAME}/.netrc
 RUN colcon mixin add default \
     https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \
     colcon mixin update || true

config/colcon-defaults.yaml

@@ -5,8 +5,6 @@ build:
     - build-testing-on
     - ccache
     - compile-commands
-  packages-ignore:
-    - btcpp_ros2_samples
 test:
   event-handlers:
     - console_direct+

pixi.toml

@@ -122,7 +122,6 @@ ros-jazzy-xacro = "*"
 # Dependencies that not available on conda-forge and must be fetched from PyPi.
 # Try not to use this section unless absolutely necessary.
 [pypi-dependencies]
-filelock = ">=3.29.1, <4"
 mujoco = "==3.12.0"
 obj2mjcf = ">=0.0.25, <0.0.26"
 pupil-apriltags = ">=1.0.4.post11, <2"

src/clr_demos/clr_behavior_demos/launch/run_roboplan_behavior.launch.xml

@@ -1,16 +1,19 @@
 <launch>
   <!-- Launch arguments -->
   <arg name="robot" default="clr" description="Robot configuration for the RoboPlan planning server." />
+  <arg name="use_sim_time" default="false" description="Whether or not to use the simulated clock." />
 
   <!-- The RoboPlan planning server. Note that the node is not renamed here since the process
        spawns several helper nodes; a launch-level name would collide them all. -->
   <node pkg="clr_behavior_demos" exec="roboplan_planning_server.py" output="screen">
     <param name="robot" value="$(var robot)" />
+    <param name="use_sim_time" value="$(var use_sim_time)" />
   </node>
 
   <!-- The main behavior tree executor -->
   <node pkg="imetro_behavior" exec="run_behavior.py" name="behavior_node" output="screen">
     <param name="behavior_config" value="$(find-pkg-share clr_behavior_demos)/config/clr_roboplan_behavior_config.yaml" />
+    <param name="use_sim_time" value="$(var use_sim_time)" />
   </node>
 
 </launch>

src/clr_demos/clr_behavior_demos/scripts/roboplan_planning_server.py

@@ -521,6 +521,7 @@ def _build_group_context(self, group_name):
                 ctx.visualizer,
                 self._traj_marker_pub,
                 ctx.q_indices,
+                clock=self.get_clock(),
             )
             self._group_contexts[group_name] = ctx
             self.get_logger().info(f"Created planning context for group '{group_name}'.")
@@ -865,11 +866,21 @@ def _plan_cartesian(self, group_name, target_poses, max_linear_speed=0.0, max_an
                 f"Planned Cartesian trajectory through {len(target_poses)} pose(s) " f"({len(traj.positions)} points)"
             )
 
+    def _stop_previews(self):
+        """
+        Stops every group's trajectory playback. Each planning context owns its
+        own player, so a preview started for one group would otherwise keep
+        publishing on the shared marker topic underneath the next group's preview.
+        """
+        for ctx in self._group_contexts.values():
+            ctx.player.stop()
+
     def _preview(self):
         if self._planned_traj is None:
             return False, "No trajectory to preview. Plan first."
 
         self.get_logger().info("Previewing trajectory...")
+        self._stop_previews()
         self._planned_ctx.player.play(
             self._planned_traj,
             self._traj_dt,
@@ -936,6 +947,7 @@ def _reset(self):
             self._target_marker_pose = None
             self._planned_traj = None
             self._planned_ctx = None
+            self._stop_previews()
             self._traj_marker_pub.publish(self._default_ctx.visualizer.clear_markers())
             delete_marker = Marker()
             delete_marker.header.frame_id = "world"

src/clr_demos/clr_behavior_demos/trees/pick_and_place_ctb.xml

@@ -199,13 +199,11 @@
             camera_info_topic="/wrist_mounted_camera/color/camera_info"
             rgb_image_topic="/wrist_mounted_camera/color/image_raw"
             depth_image_topic="/wrist_mounted_camera/aligned_depth_to_color/image_raw"
-            point_cloud_topic="/wrist_mounted_camera/depth/color/points"
             time_slop="0.1"
             sync_timeout="3.0"
             camera_info="{camera_info}"
             rgb_image="{rgb_image}"
             depth_image="{depth_image}"
-            point_cloud="{point_cloud}"
           />
       <!-- </Parallel> -->
       <DetectColorBlobs

src/clr_demos/clr_behavior_demos/trees/roboplan/perception_trees.xml

@@ -8,13 +8,11 @@
         camera_info_topic="/wrist_mounted_camera/color/camera_info"
         rgb_image_topic="/wrist_mounted_camera/color/image_raw"
         depth_image_topic="/wrist_mounted_camera/aligned_depth_to_color/image_raw"
-        point_cloud_topic="/wrist_mounted_camera/depth/color/points"
         time_slop="0.1"
         sync_timeout="3.0"
         camera_info="{camera_info}"
         rgb_image="{rgb_image}"
         depth_image="{depth_image}"
-        point_cloud="{point_cloud}"
       />
       <DetectColorBlobs
         name="detect_color_blobs"

src/clr_demos/clr_roboplan_demos/CMakeLists.txt

@@ -6,7 +6,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 find_package(ament_cmake REQUIRED)
-find_package(roboplan REQUIRED)
+find_package(roboplan_core REQUIRED)
 find_package(roboplan_simple_ik REQUIRED)
 find_package(roboplan_oink REQUIRED)
 find_package(roboplan_rrt REQUIRED)

src/clr_demos/clr_roboplan_demos/clr_roboplan_demos/__init__.py

@@ -35,7 +35,7 @@
     QoSDurabilityPolicy,
 )
 
-from roboplan.core import Scene
+from roboplan.core import Scene, loadJointLimitsConfig, loadUrdfSceneDescriptionFromXml
 
 
 BEST_EFFORT_QOS = QoSProfile(
@@ -148,9 +148,8 @@ def create_scene(urdf_xml=None):
 
     scene = Scene(
         name="clr_scene",
-        urdf=urdf_xml,
-        srdf=srdf_xml,
-        package_paths=package_paths,
-        yaml_config_path=yaml_config_path,
+        description=loadUrdfSceneDescriptionFromXml(urdf_xml, package_paths),
     )
+    scene.importJointLimitsFromConfig(loadJointLimitsConfig(yaml_config_path))
+    scene.importSrdf(srdf_xml)
     return scene, urdf_xml, package_paths

src/clr_demos/clr_roboplan_demos/scripts/plan_and_execute_node.py

@@ -230,6 +230,7 @@ def ik_solve_fn(target_pose, seed):
             self._traj_visualizer,
             self._traj_marker_pub,
             self._q_indices,
+            self.get_clock(),
         )
 
         # Publish the planned end-effector path as a light green line upon planning.

src/clr_demos/clr_roboplan_demos/scripts/viser_ik_rrt.py

@@ -18,6 +18,8 @@
     PathShortcutter,
     PathShortcuttingOptions,
     Scene,
+    loadJointLimitsConfig,
+    loadUrdfSceneDescriptionFromXml,
 )
 from roboplan.rrt import RRTOptions, RRT, visualizeTree
 from roboplan.simple_ik import SimpleIkOptions, SimpleIk
@@ -109,11 +111,10 @@ def main(
     ]
     scene = Scene(
         "test_scene",
-        urdf=urdf_xml,
-        srdf=srdf_xml,
-        package_paths=package_paths,
-        yaml_config_path=yaml_config_path,
+        description=loadUrdfSceneDescriptionFromXml(urdf_xml, package_paths),
     )
+    scene.importJointLimitsFromConfig(loadJointLimitsConfig(yaml_config_path))
+    scene.importSrdf(srdf_xml)
 
     group_name = "clr"
     tip_link = "grasp_frame"