Add control for octomap.

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

dockerfiles/install_scripts/dev_packages.sh

@@ -10,4 +10,5 @@ apt update
 
 apt install -y \
     ros-$ROS_DISTRO-nmea-navsat-driver \
-    ros-$ROS_DISTRO-moveit-ros-perception
+    ros-$ROS_DISTRO-moveit-ros-perception \
+    ros-$ROS_DISTRO-topic-tools

ros2_ws/src/rcdt_franka_moveit_config/config/sensors_3d.yaml

@@ -1,12 +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: /realsense1/depth/image_rect_raw
+  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: 1.0
-  filtered_cloud_topic: /realsense1/filtered_points
+  max_update_rate: 10.0
+  filtered_cloud_topic: ""

ros2_ws/src/rcdt_launch/rcdt_launch/moveit.py

@@ -57,6 +57,14 @@ def add(namespace: str, robot_description: dict, platform: str) -> None:
         )
         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

ros2_ws/src/rcdt_launch/rcdt_launch/robot.py

@@ -487,7 +487,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:
@@ -883,10 +887,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

@@ -175,7 +175,7 @@ def add_planning_scene(namespace: str) -> None:
                 "Move Group Namespace": namespace,
                 "Robot Description": f"{namespace}_robot_description",
                 "Name": f"{namespace}_planning_scene",
-                "Planning Scene Topic": f"/{namespace}/planning_scene",
+                "Planning Scene Topic": f"/{namespace}/monitored_planning_scene",
             }
         )
 

ros2_ws/src/rcdt_moveit/include/moveit_manager.hpp

@@ -3,6 +3,8 @@
 // # SPDX-License-Identifier: Apache-2.0
 
 #include "geometry_msgs/msg/transform_stamped.hpp"
+#include <boost/process.hpp>
+#include <boost/process/group.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <moveit/move_group_interface/move_group_interface.hpp>
 #include <moveit/planning_scene_interface/planning_scene_interface.hpp>
@@ -19,6 +21,7 @@
 #include <rcdt_messages/srv/transform_goal_pose.hpp>
 #include <rclcpp/node.hpp>
 #include <rviz_visual_tools/rviz_visual_tools.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #include <string>
 #include <tf2_ros/transform_broadcaster.h>
@@ -33,6 +36,7 @@ typedef rcdt_messages::srv::MoveHandToPose MoveHandToPose;
 typedef rcdt_messages::srv::PoseStampedSrv PoseStampedSrv;
 typedef moveit_msgs::srv::ServoCommandType ServoCommandType;
 typedef std_srvs::srv::Trigger Trigger;
+typedef std_srvs::srv::SetBool SetBool;
 typedef geometry_msgs::msg::PoseStamped PoseStamped;
 typedef geometry_msgs::msg::TransformStamped TransformStamped;
 
@@ -55,12 +59,16 @@ class MoveitManager {
   const moveit::core::JointModelGroup *jmg_arm;
   const moveit::core::JointModelGroup *jmg_hand;
   const moveit::core::JointModelGroup *jmg_tcp;
+  std::string namespace_arm;
+  std::string namespace_camera;
   std::string base_frame = "map";
   std::string marker_topic = "/rviz_markers";
   moveit::planning_interface::MoveGroupInterface::Plan plan;
   rviz_visual_tools::RvizVisualTools rviz_visual_tools;
   moveit_visual_tools::MoveItVisualTools moveit_visual_tools;
   PoseStamped goal_pose;
+  boost::process::child process;
+  boost::process::group process_group;
 
   //   Definitions:
   std::map<std::string, int> shapes = {
@@ -80,6 +88,10 @@ class MoveitManager {
   void clear_objects(const std::shared_ptr<Trigger::Request> request,
                      std::shared_ptr<Trigger::Response> response);
 
+  rclcpp::Service<SetBool>::SharedPtr toggle_octomap_scan_service;
+  void toggle_octomap_scan(const std::shared_ptr<SetBool::Request> request,
+                           std::shared_ptr<SetBool::Response> response);
+
   rclcpp::Service<DefineGoalPose>::SharedPtr define_goal_pose_service;
   void define_goal_pose(const std::shared_ptr<DefineGoalPose::Request> request,
                         std::shared_ptr<DefineGoalPose::Response> response);

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

@@ -9,7 +9,8 @@
 from rcdt_utilities.launch_utils import LaunchArgument
 from rcdt_utilities.register import Register
 
-namespace_arg = LaunchArgument("namespace", "")
+namespace_arm_arg = LaunchArgument("namespace_arm", "")
+namespace_camera_arg = LaunchArgument("namespace_camera", "")
 
 
 def launch_setup(context: LaunchContext) -> list:
@@ -21,8 +22,9 @@ def launch_setup(context: LaunchContext) -> list:
     Returns:
         list: A list of actions to be executed in the launch description.
     """
-    namespace = namespace_arg.string_value(context)
-    configuration = Moveit.configurations[namespace]
+    namespace_arm = namespace_arm_arg.string_value(context)
+    namespace_camera = namespace_camera_arg.string_value(context)
+    configuration = Moveit.configurations[namespace_arm]
 
     # Parameters required for move_group:
     move_group_parameters = []
@@ -34,48 +36,50 @@ def launch_setup(context: LaunchContext) -> list:
     move_group_parameters.append(configuration.planning_pipelines)
     move_group_parameters.append(configuration.pilz_cartesian_limits)
     move_group_parameters.append(configuration.sensors_3d)
+    move_group_parameters.append({"octomap_resolution": 0.02})
 
     # Parameters required for moveit_manager:
     moveit_manager_parameters = []
     moveit_manager_parameters.append(configuration.robot_description)
     moveit_manager_parameters.append(configuration.robot_description_semantic)
     moveit_manager_parameters.append(configuration.robot_description_kinematics)
+    moveit_manager_parameters.append({"namespace_camera": namespace_camera})
 
     # Parameters required for moveit_servo:
     moveit_servo_parameters = []
     moveit_servo_parameters.append(configuration.robot_description)
     moveit_servo_parameters.append(configuration.robot_description_semantic)
     moveit_servo_parameters.append(configuration.robot_description_kinematics)
-    moveit_servo_parameters.append(Moveit.servo_configurations[namespace])
+    moveit_servo_parameters.append(Moveit.servo_configurations[namespace_arm])
 
     move_group = Node(
         package="moveit_ros_move_group",
         executable="move_group",
         parameters=move_group_parameters,
-        namespace=namespace,
+        namespace=namespace_arm,
     )
 
     moveit_manager = Node(
         package="rcdt_moveit",
         executable="moveit_manager",
         output="screen",
         parameters=moveit_manager_parameters,
-        namespace=namespace,
+        namespace=namespace_arm,
     )
 
     moveit_servo = Node(
         package="moveit_servo",
         executable="servo_node",
         parameters=moveit_servo_parameters,
-        namespace=namespace,
+        namespace=namespace_arm,
     )
 
     switch_servo_type_to_twist = ExecuteProcess(
         cmd=[
             "ros2",
             "service",
             "call",
-            f"/{namespace}/servo_node/switch_command_type",
+            f"/{namespace_arm}/servo_node/switch_command_type",
             "moveit_msgs/srv/ServoCommandType",
             "{command_type: 1}",
         ]
@@ -101,7 +105,8 @@ def generate_launch_description() -> LaunchDescription:
     """
     return LaunchDescription(
         [
-            namespace_arg.declaration,
+            namespace_arm_arg.declaration,
+            namespace_camera_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]
     )

ros2_ws/src/rcdt_moveit/launch/relay_octomap.launch.py

@@ -0,0 +1,66 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+from launch import LaunchContext, LaunchDescription
+from launch.actions import OpaqueFunction
+from launch_ros.actions import Node
+from rcdt_utilities.launch_utils import LaunchArgument
+
+namespace_arm_arg = LaunchArgument("namespace_arm", "")
+namespace_camera_arg = LaunchArgument("namespace_camera", "")
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """This launch file starts two topic_tools relay nodes to pass depth images and camera info from a camera to an arm.
+
+    A move_group of the arm can use these topics to build an octomap.
+    This launch file is launched as a separate process from the MoveitManager class.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Returns:
+        list: A list of actions to be executed in the launch description.
+    """
+    namespace_arm = namespace_arm_arg.string_value(context)
+    namespace_camera = namespace_camera_arg.string_value(context)
+
+    relay_depth_image = Node(
+        package="topic_tools",
+        executable="relay",
+        name="relay_depth_image",
+        namespace=namespace_arm,
+        arguments=[
+            f"/{namespace_camera}/depth/image_rect_raw",
+            f"/{namespace_arm}/octomap/depth_image",
+        ],
+    )
+
+    relay_camera_info = Node(
+        package="topic_tools",
+        executable="relay",
+        name="relay_camera_info",
+        namespace=namespace_arm,
+        arguments=[
+            f"/{namespace_camera}/depth/camera_info",
+            f"/{namespace_arm}/octomap/camera_info",
+        ],
+    )
+
+    return [relay_depth_image, relay_camera_info]
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generate the launch description.
+
+    Returns:
+        LaunchDescription: The launch description.
+    """
+    return LaunchDescription(
+        [
+            namespace_arm_arg.declaration,
+            namespace_camera_arg.declaration,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

ros2_ws/src/rcdt_moveit/src/moveit_manager.cpp

@@ -8,14 +8,15 @@
 #include <rclcpp/executors/multi_threaded_executor.hpp>
 #include <rclcpp/logging.hpp>
 #include <rclcpp/node_options.hpp>
+#include <rclcpp/parameter_value.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <vector>
 
 using std::placeholders::_1;
 using std::placeholders::_2;
 
 MoveitManager::MoveitManager(rclcpp::Node::SharedPtr node_)
-    : node(node_), tf_broadcaster(node_),
+    : node(node_), tf_broadcaster(node),
       move_group(
           node,
           moveit::planning_interface::MoveGroupInterface::Options(
@@ -25,6 +26,9 @@ MoveitManager::MoveitManager(rclcpp::Node::SharedPtr node_)
       rviz_visual_tools(base_frame, marker_topic, node),
       moveit_visual_tools(node, base_frame, marker_topic) {
 
+  namespace_arm = std::string(node->get_namespace()).erase(0, 1);
+  namespace_camera = node->get_parameter("namespace_camera").as_string();
+
   jmg_arm = move_group.getRobotModel()->getJointModelGroup("arm");
   jmg_hand = move_group.getRobotModel()->getJointModelGroup("hand");
   jmg_tcp = move_group.getRobotModel()->getJointModelGroup("tcp");
@@ -55,6 +59,10 @@ void MoveitManager::initialize_services() {
       "~/clear_objects",
       std::bind(&MoveitManager::clear_objects, this, _1, _2));
 
+  toggle_octomap_scan_service = node->create_service<SetBool>(
+      "~/toggle_octomap_scan",
+      std::bind(&MoveitManager::toggle_octomap_scan, this, _1, _2));
+
   define_goal_pose_service = node->create_service<DefineGoalPose>(
       "~/define_goal_pose",
       std::bind(&MoveitManager::define_goal_pose, this, _1, _2));
@@ -126,6 +134,45 @@ void MoveitManager::clear_objects(
   response->success = true;
 };
 
+/**
+ * Toggle the octomap scan by running a launch file with topic_tools relay nodes
+ * as a boost process. These relay nodes pass the depth_image and
+ * camera_info to the topic where MoveIt octomap is subscribed. We use
+ * topic_tools relay for performance, since this does not perform serialization
+ * on the messages. We create a process group and terminate the whole group
+ * instead of the process, since terminating a single process may leave child
+ * processes running.
+ */
+void MoveitManager::toggle_octomap_scan(
+    const std::shared_ptr<SetBool::Request> request,
+    std::shared_ptr<SetBool::Response> response) {
+  if (namespace_camera.empty()) {
+    RCLCPP_ERROR(node->get_logger(),
+                 "Namespace of camera is not set. Cannot toggle octomap scan.");
+    return;
+  }
+  auto cmd = fmt::format("ros2 launch rcdt_moveit relay_octomap.launch.py "
+                         "namespace_arm:={} namespace_camera:={}",
+                         namespace_arm, namespace_camera);
+  if (request->data) {
+    if (process.running()) {
+      RCLCPP_WARN(node->get_logger(),
+                  "Cannot enable octomap scan: already active.");
+      return;
+    }
+    process = boost::process::child(cmd, process_group);
+    RCLCPP_INFO(node->get_logger(), "Octomap scan enabled.");
+  } else {
+    if (!process.running()) {
+      RCLCPP_WARN(node->get_logger(),
+                  "Cannot disable octomap scan: not active.");
+      return;
+    }
+    process_group.terminate();
+    RCLCPP_INFO(node->get_logger(), "Octomap scan disabled.");
+  }
+};
+
 void MoveitManager::define_goal_pose(
     const std::shared_ptr<DefineGoalPose::Request> request,
     std::shared_ptr<DefineGoalPose::Response> response) {