Introduce creation of octomap.

Signed-off-by: Jelmer de Wolde <[email protected]>

dockerfiles/install_scripts/dev_packages.sh

@@ -9,4 +9,5 @@ source /home/$UNAME/.bashrc
 apt update
 
 apt install -y \
-    ros-$ROS_DISTRO-nmea-navsat-driver
+    ros-$ROS_DISTRO-nmea-navsat-driver \
+    ros-$ROS_DISTRO-moveit-ros-perception

ros2_ws/src/rcdt_franka_moveit_config/config/sensors_3d.yaml

@@ -0,0 +1,12 @@
+sensors:
+  - depth_image
+depth_image:
+  sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
+  image_topic: /realsense1/depth/image_rect_raw
+  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: 1.0
+  filtered_cloud_topic: /realsense1/filtered_points

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

@@ -94,7 +94,8 @@ def launch_setup(context: LaunchContext) -> list:  # noqa: PLR0912, PLR0915
             Rviz.add_markers()
             Rviz.load_robot_state = True
             Rviz.load_trajectory = True
-            arm = Arm("franka", [0, 0, 0], moveit=True, graspnet=True)
+            Rviz.load_planning_scene = True
+            arm = Arm("franka", [0, 0, 0], moveit=True, graspnet=False)
             Camera("realsense", [0.05, 0, 0], [0, -90, 180], parent=arm)
         case "panther":
             Vehicle("panther", [0, 0, 0.2], namespace="panther")

ros2_ws/src/rcdt_launch/rcdt_launch/moveit.py

@@ -52,6 +52,9 @@ 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()
 
         # adapt robot_description with prefix:

ros2_ws/src/rcdt_moveit/launch/moveit.launch.py

@@ -33,6 +33,7 @@ def launch_setup(context: LaunchContext) -> list:
     move_group_parameters.append(configuration.trajectory_execution)
     move_group_parameters.append(configuration.planning_pipelines)
     move_group_parameters.append(configuration.pilz_cartesian_limits)
+    move_group_parameters.append(configuration.sensors_3d)
 
     # Parameters required for moveit_manager:
     moveit_manager_parameters = []