Add octomap and visualizations to moveit and rviz.

.github/workflows/reusable-workspace.yml

@@ -60,7 +60,7 @@ jobs:
             --entrypoint /bin/bash "${{ inputs.container-image }}" -i -c "
               source /home/rcdt/.bashrc &&
               source install/setup.bash &&
-              uv run pytest -s -rsxf src/
+              uv run xvfb-run pytest -s -rsxf src/
             "
 
       - name: Show disk usage after job

dockerfiles/install_scripts/dev_packages.sh

@@ -11,4 +11,7 @@ apt update
 apt install -y \
     clang \
     doxygen \
-    ros-$ROS_DISTRO-nmea-navsat-driver
+    xvfb \
+    ros-$ROS_DISTRO-nmea-navsat-driver \
+    ros-$ROS_DISTRO-moveit-ros-perception \
+    ros-$ROS_DISTRO-topic-tools

pyproject.toml

@@ -21,6 +21,7 @@ dependencies = [
     "lark>=1.2.2",
     "mashumaro>=3.16",
     "myst-parser>=4.0.1",
+    "nicegui>=3.1.0",
     "numpy<2.0",
     "opencv-python>=4.11.0.86",
     "pillow>=11.3.0",

ros2_ws/src/rcdt_franka_moveit_config/config/fr3.srdf

@@ -28,7 +28,7 @@ are defined
     group-->
     <!--SUBGROUPS:
     Groups can also be formed by referencing to already defined group names-->
-    <group name="fr3_arm">
+    <group name="arm">
         <joint name="fr3_joint1" />
         <joint name="fr3_joint2" />
         <joint name="fr3_joint3" />
@@ -37,10 +37,20 @@ are defined
         <joint name="fr3_joint6" />
         <joint name="fr3_joint7" />
     </group>
+    <group name="hand">
+        <joint name="fr3_joint8" />
+        <joint name="fr3_hand_joint" />
+        <joint name="fr3_hand_tcp_joint" />
+        <joint name="fr3_finger_joint1" />
+        <joint name="fr3_finger_joint2" />
+    </group>
+    <group name="tcp">
+        <link name="fr3_hand_tcp" />
+    </group>
     <!--GROUP_STATES:
     Purpose: Define a named state for a particular group, in terms of joint values. This is
     useful to define states like 'folded arms'-->
-    <group_state name="home" group="fr3_arm">
+    <group_state name="home" group="arm">
         <joint name="fr3_joint1" value="0" />
         <joint name="fr3_joint2" value="-0.78539816339" />
         <joint name="fr3_joint3" value="0" />
@@ -49,7 +59,7 @@ are defined
         <joint name="fr3_joint6" value="1.57079632679" />
         <joint name="fr3_joint7" value="0.78539816339" />
     </group_state>
-    <group_state name="drop" group="fr3_arm">
+    <group_state name="drop" group="arm">
         <joint name="fr3_joint1" value="-1.57079632679" />
         <joint name="fr3_joint2" value="-0.65" />
         <joint name="fr3_joint3" value="0" />
@@ -61,6 +71,7 @@ are defined
 
     <!--END_EFFECTOR:
     Purpose: Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="fr3_link7" group="hand" parent_group="arm" />
     <!--DISABLE_COLLISIONS:
     By default it is assumed that any link of the robot could potentially come into
     collision with any other link in the robot. This tag disables collision checking between a

ros2_ws/src/rcdt_franka_moveit_config/config/kinematics.yaml

@@ -2,7 +2,7 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-fr3_arm:
+arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.0050000000000000001
   kinematics_solver_timeout: 0.0050000000000000001

ros2_ws/src/rcdt_franka_moveit_config/config/sensors_3d.yaml

@@ -0,0 +1,16 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+sensors:
+  - depth_image
+depth_image:
+  sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+  image_topic: ""
+  queue_size: 5
+  near_clipping_plane_distance: 0.3
+  far_clipping_plane_distance: 5.0
+  shadow_threshold: 0.2
+  padding_scale: 1.0
+  max_update_rate: 10.0
+  filtered_cloud_topic: ""

ros2_ws/src/rcdt_franka_moveit_config/config/servo_params.yaml

@@ -7,7 +7,7 @@
 thread_priority: 40
 publish_period: 0.001
 max_expected_latency: 0.1
-move_group_name: fr3_arm
+move_group_name: arm
 active_subgroup: ""
 
 ############################# INCOMING COMMAND SETTINGS ########################

ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py

@@ -123,6 +123,7 @@ def generate_launch_description() -> LaunchDescription:
             world_arg.declaration,
             platforms_arg.declaration,
             positions_arg.declaration,
+            orientations_arg.declaration,
             bridge_topics_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]

ros2_ws/src/rcdt_launch/launch/robots.launch.py

@@ -29,6 +29,7 @@
         "franka",
         "franka_axis",
         "franka_double",
+        "franka_planning",
         "franka_realsense",
         "panther",
         "panther_axis",
@@ -86,6 +87,14 @@ def launch_setup(context: LaunchContext) -> list:  # noqa: PLR0912, PLR0915
             if not use_sim:
                 Rviz.load_motion_planning_plugin = True
             Arm("franka", [0, 0, 0], gripper=True, moveit=True, ip_address="172.16.0.2")
+        case "franka_planning":
+            Platform.world = "table_with_1_brick.sdf"
+            Rviz.add_markers()
+            Rviz.load_robot_state = True
+            Rviz.load_trajectory = True
+            Rviz.load_planning_scene = True
+            arm = Arm("franka", [0, 0, 0], moveit=True)
+            Camera("realsense", [0.05, 0, 0], [0, -90, 180], parent=arm)
         case "franka_axis":
             franka = Arm("franka", [0, 0, 0], [0, 0, 20])
             Platform("axis", [0, 0, 0.1], [0, 20, 0], parent=franka)

ros2_ws/src/rcdt_launch/rcdt_launch/moveit.py

@@ -52,8 +52,19 @@ def add(namespace: str, robot_description: dict, platform: str) -> None:
         moveit_config_builder.moveit_cpp(
             get_file_path(package, ["config"], "planning_pipeline.yaml")
         )
+        moveit_config_builder.sensors_3d(
+            get_file_path(package, ["config"], "sensors_3d.yaml")
+        )
         moveit_config = moveit_config_builder.to_moveit_configs()
 
+        # Define namespace dependent parameters:
+        moveit_config.sensors_3d["depth_image"]["image_topic"] = (
+            f"/{namespace}/octomap/depth_image"
+        )
+        moveit_config.sensors_3d["depth_image"]["filtered_cloud_topic"] = (
+            f"/{namespace}/octomap/filtered_points"
+        )
+
         # adapt robot_description with prefix:
         add_prefix_in_robot_description(robot_description, namespace)
         moveit_config.robot_description = robot_description
@@ -96,6 +107,11 @@ def add_prefix_in_robot_description_semantic(description: dict, prefix: str) ->
         prefix (str): The prefix to add to each link.
     """
     xml_dict = xmltodict.parse(description["robot_description_semantic"])
+    ee_parent_link = xml_dict["robot"]["end_effector"]["@parent_link"]
+    xml_dict["robot"]["end_effector"]["@parent_link"] = f"{prefix}/{ee_parent_link}"
+    for group in xml_dict["robot"]["group"]:
+        if "link" in group:
+            group["link"]["@name"] = f"{prefix}/{group['link']['@name']}"
     for disable_collision in xml_dict["robot"]["disable_collisions"]:
         link1 = disable_collision["@link1"]
         link2 = disable_collision["@link2"]

ros2_ws/src/rcdt_launch/rcdt_launch/robot.py

@@ -21,13 +21,15 @@ class Platform:  # noqa: PLR0904
 
     Attributes:
         simulation (bool): Whether the platforms are in simulation mode or not.
+        world (str): The world file to be used in Gazebo.
         platforms (list[Platform]): A list of all the platforms.
         platform_indices (dict[str, int]): A collections of the different platforms and the number of occurrences.
         names (list[str]): A list of all robot names.
         bridge_topics (list[str]): A list of all topics that should be bridged between Gazebo and ROS.
     """
 
     simulation: bool = True
+    world: str = "walls.sdf"
     platforms: list["Platform"] = []
     platform_indices: dict[str, int] = {}
     names: list[str] = []
@@ -141,6 +143,7 @@ def create_gazebo_launch(load_gazebo_ui: bool) -> RegisteredLaunchDescription:
             get_file_path("rcdt_gazebo", ["launch"], "gazebo_robot.launch.py"),
             launch_arguments={
                 "load_gazebo_ui": str(load_gazebo_ui),
+                "world": Platform.world,
                 "platforms": " ".join(platforms),
                 "positions": " ".join(positions),
                 "orientations": " ".join(orientations),
@@ -506,7 +509,11 @@ def create_state_publisher(self) -> Node | None:
             package="robot_state_publisher",
             executable="robot_state_publisher",
             namespace=self.namespace,
-            parameters=[self.robot_description, {"frame_prefix": self.frame_prefix}],
+            parameters=[
+                self.robot_description,
+                {"frame_prefix": self.frame_prefix},
+                {"publish_frequency": 1000.0},
+            ],
         )
 
     def create_parent_link(self) -> Node:
@@ -832,7 +839,11 @@ def __init__(  # noqa: PLR0913
 
         if moveit:
             Moveit.add(self.namespace, self.robot_description, self.platform)
+            Rviz.moveit_namespaces.append(self.namespace)
             Rviz.add_motion_planning_plugin(self.namespace)
+            Rviz.add_planning_scene(self.namespace)
+            Rviz.add_robot_state(self.namespace)
+            Rviz.add_trajectory(self.namespace)
 
     def create_launch_description(self) -> list[RegisteredLaunchDescription]:
         """Create the launch description with specific elements for an arm.
@@ -905,10 +916,8 @@ def create_moveit_launch(self) -> RegisteredLaunchDescription:
         return RegisteredLaunchDescription(
             get_file_path("rcdt_moveit", ["launch"], "moveit.launch.py"),
             launch_arguments={
-                "robot_name": "fr3",
-                "moveit_package_name": "rcdt_franka_moveit_config",
-                "servo_params_package": "rcdt_franka",
-                "namespace": self.namespace,
+                "namespace_arm": self.namespace,
+                "namespace_camera": self.camera.namespace if self.camera else "",
             },
         )
 

ros2_ws/src/rcdt_launch/rcdt_launch/rviz.py

@@ -13,13 +13,19 @@ class Rviz:
         yaml (dict): The default RViz configuration.
         displays (list): The list of displays in the RViz configuration.
         load_motion_planning_plugin (bool): Whether to load the motion planning plugin.
+        load_planning_scene (bool): Whether to load the planning scene display.
+        load_robot_state (bool): Whether to load the robot state display.
+        load_trajectory (bool): Whether to load the trajectory display.
         load_point_cloud (bool): Whether to load point cloud displays.
         moveit_namespaces (list[str]): A list of the namespaces where MoveIt is launched.
     """
 
     yaml: dict = get_yaml(get_file_path("rcdt_utilities", ["rviz"], "default.rviz"))
     displays: list = yaml["Visualization Manager"]["Displays"]
     load_motion_planning_plugin: bool = False
+    load_planning_scene: bool = False
+    load_robot_state: bool = False
+    load_trajectory: bool = False
     load_point_cloud: bool = False
     moveit_namespaces: list[str] = []
 
@@ -142,7 +148,6 @@ def add_motion_planning_plugin(namespace: str) -> None:
         """
         if not Rviz.load_motion_planning_plugin:
             return
-        Rviz.moveit_namespaces.append(namespace)
         Rviz.displays.append(
             {
                 "Enabled": True,
@@ -154,6 +159,67 @@ def add_motion_planning_plugin(namespace: str) -> None:
             }
         )
 
+    @staticmethod
+    def add_planning_scene(namespace: str) -> None:
+        """Add the planning scene display to the RViz configuration.
+
+        Args:
+            namespace (str): The namespace of the robot.
+        """
+        if not Rviz.load_planning_scene:
+            return
+        Rviz.displays.append(
+            {
+                "Enabled": True,
+                "Class": "moveit_rviz_plugin/PlanningScene",
+                "Move Group Namespace": namespace,
+                "Robot Description": f"{namespace}_robot_description",
+                "Name": f"{namespace}_planning_scene",
+                "Planning Scene Topic": f"/{namespace}/monitored_planning_scene",
+                "Scene Robot": {"Show Robot Visual": False},
+            }
+        )
+
+    @staticmethod
+    def add_robot_state(namespace: str) -> None:
+        """Add the robot state display to the RViz configuration.
+
+        Args:
+            namespace (str): The namespace of the robot.
+        """
+        if not Rviz.load_robot_state:
+            return
+        Rviz.displays.append(
+            {
+                "Enabled": True,
+                "Class": "moveit_rviz_plugin/RobotState",
+                "Robot Description": f"{namespace}_robot_description",
+                "Robot State Topic": f"/{namespace}/display_robot_state",
+                "Name": f"{namespace}_robot_state",
+                "TF Prefix": namespace,
+            }
+        )
+
+    @staticmethod
+    def add_trajectory(namespace: str) -> None:
+        """Add the trajectory display to the RViz configuration.
+
+        Args:
+            namespace (str): The namespace of the robot.
+        """
+        if not Rviz.load_trajectory:
+            return
+        Rviz.displays.append(
+            {
+                "Enabled": True,
+                "Class": "moveit_rviz_plugin/Trajectory",
+                "Robot Description": f"{namespace}_robot_description",
+                "Name": f"{namespace}_trajectory",
+                "Trajectory Topic": f"/{namespace}/display_planned_path_custom",
+                "State Display Time": "0.5s",
+            }
+        )
+
     @staticmethod
     def add_map(topic: str) -> None:
         """Add a map to the RViz configuration.
@@ -205,6 +271,22 @@ def add_polygon(topic: str) -> None:
             }
         )
 
+    @staticmethod
+    def add_markers(topic: str = "/rviz_markers") -> None:
+        """Add a MarkerArray display (e.g., for MoveItVisualTools).
+
+        Args:
+            topic (str): The topic of the MarkerArray.
+        """
+        Rviz.displays.append(
+            {
+                "Enabled": True,
+                "Class": "rviz_default_plugins/MarkerArray",
+                "Name": topic,
+                "Topic": {"Value": topic},
+            }
+        )
+
     @staticmethod
     def create_rviz_file() -> None:
         """Create the RViz configuration file."""

ros2_ws/src/rcdt_messages/srv/PoseStampedSrv.srv

@@ -0,0 +1,8 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+
+geometry_msgs/PoseStamped pose
+---
+bool success