srdf_publisher_node

moveit_ros/planning/CMakeLists.txt

@@ -16,7 +16,9 @@ find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_occupancy_map_monitor REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(srdfdom REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
@@ -59,6 +61,7 @@ set(THIS_PACKAGE_LIBRARIES
   moveit_robot_model_loader
   moveit_trajectory_execution_manager
   planning_pipeline_parameters
+  srdf_publisher_node
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -70,7 +73,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   moveit_ros_occupancy_map_monitor
   pluginlib
   rclcpp
+  rclcpp_components
   srdfdom
+  std_msgs
   tf2
   tf2_eigen
   tf2_geometry_msgs
@@ -97,6 +102,7 @@ add_subdirectory(planning_response_adapter_plugins)
 add_subdirectory(planning_scene_monitor)
 add_subdirectory(rdf_loader)
 add_subdirectory(robot_model_loader)
+add_subdirectory(srdf_publisher_node)
 add_subdirectory(trajectory_execution_manager)
 
 install(
@@ -111,6 +117,10 @@ install(
 ament_export_targets(moveit_ros_planningTargets HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+rclcpp_components_register_node(srdf_publisher_node
+  PLUGIN "moveit_ros_planning::SrdfPublisher"
+  EXECUTABLE srdf_publisher)
+
 pluginlib_export_plugin_description_file(moveit_core "default_request_adapters_plugin_description.xml")
 pluginlib_export_plugin_description_file(moveit_core "default_response_adapters_plugin_description.xml")
 if(BUILD_TESTING)

moveit_ros/planning/package.xml

@@ -34,8 +34,10 @@
   <depend>moveit_msgs</depend>
   <depend>moveit_ros_occupancy_map_monitor</depend>
   <depend>rclcpp_action</depend>
+  <depend>rclcpp_components</depend>
   <depend>rclcpp</depend>
   <depend>srdfdom</depend>
+  <depend>std_msgs</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_msgs</depend>

moveit_ros/planning/srdf_publisher_node/CMakeLists.txt

@@ -0,0 +1,14 @@
+add_library(srdf_publisher_node SHARED src/srdf_publisher_node.cpp)
+ament_target_dependencies(srdf_publisher_node PUBLIC
+    std_msgs
+    rclcpp
+    rclcpp_components
+    )
+
+if(BUILD_TESTING)
+    find_package(launch_testing_ament_cmake)
+
+    add_launch_test(test/srdf_publisher_test.py
+        TARGET test-srdf_publisher
+    )
+endif()

moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp

@@ -0,0 +1,95 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, PickNik Robotics Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Robotics Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+// SrdfPublisher Node
+// Author: Tyler Weaver
+//
+// Node with a single parameter and single publisher using transient local QoS.
+// Publishes string set on parameter `robot_description_semantic` to topic
+// `/robot_description_semantic`.
+//
+// This is similar to what [robot_state_publisher](https://github.com/ros/robot_state_publisher)
+// does for the URDF using parameter `robot_description` and topic `/robot_description`.
+//
+// As MoveIt subscribes to the topic `/robot_description_semantic` for the srdf
+// this node can be used when you want to set the SRDF parameter on only one node
+// and have the rest of your system use a subscriber to that topic to get the
+// SRDF.
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <string>
+
+namespace moveit_ros_planning
+{
+
+class SrdfPublisher : public rclcpp::Node
+{
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr srdf_publisher_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_handle_;
+
+public:
+  SrdfPublisher(const rclcpp::NodeOptions& options) : rclcpp::Node("srdf_publisher", options)
+  {
+    srdf_publisher_ = this->create_publisher<std_msgs::msg::String>("robot_description_semantic",
+                                                                    rclcpp::QoS(1).transient_local().reliable());
+    on_set_parameters_handle_ =
+        this->add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter>& parameters) {
+          for (auto const& parameter : parameters)
+          {
+            if (parameter.get_name() == "robot_description_semantic")
+            {
+              std_msgs::msg::String msg;
+              msg.data = parameter.get_value<std::string>();
+              srdf_publisher_->publish(msg);
+            }
+          }
+
+          rcl_interfaces::msg::SetParametersResult result;
+          result.successful = true;
+          return result;
+        });
+
+    rcl_interfaces::msg::ParameterDescriptor descriptor;
+    descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
+    descriptor.description = "Semantic Robot Description Format";
+    this->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING, descriptor);
+  }
+};
+
+}  // namespace moveit_ros_planning
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(moveit_ros_planning::SrdfPublisher)

moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py

@@ -0,0 +1,124 @@
+# Software License Agreement (BSD License)
+#
+#  Copyright (c) 2024, PickNik Robotics Inc.
+#  All rights reserved.
+#
+#  Redistribution and use in source and binary forms, with or without
+#  modification, are permitted provided that the following conditions
+#  are met:
+#
+#  # Redistributions of source code must retain the above copyright
+#     notice, this list of conditions and the following disclaimer.
+#  # Redistributions in binary form must reproduce the above
+#     copyright notice, this list of conditions and the following
+#     disclaimer in the documentation and/or other materials provided
+#     with the distribution.
+#  # Neither the name of PickNik Robotics Inc. nor the names of its
+#     contributors may be used to endorse or promote products derived
+#     from this software without specific prior written permission.
+#
+#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#  POSSIBILITY OF SUCH DAMAGE.
+
+# Author: Tyler Weaver
+
+import unittest
+from threading import Event
+from threading import Thread
+
+import launch
+import launch_ros
+import launch_testing
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import (
+    QoSProfile,
+    QoSReliabilityPolicy,
+    QoSDurabilityPolicy,
+)
+from std_msgs.msg import String
+
+import pytest
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    srdf_publisher = launch_ros.actions.Node(
+        package="moveit_ros_planning",
+        parameters=[{"robot_description_semantic": "test value"}],
+        executable="srdf_publisher",
+        output="screen",
+    )
+
+    return launch.LaunchDescription(
+        [
+            srdf_publisher,
+            # Start tests right away - no need to wait for anything
+            launch_testing.actions.ReadyToTest(),
+        ]
+    ), {"srdf_publisher": srdf_publisher}
+
+
+class SrdfObserverNode(Node):
+    def __init__(self, name="srdf_observer_node"):
+        super().__init__(name)
+        self.msg_event_object = Event()
+        self.srdf = ""
+
+    def start_subscriber(self):
+        # Create a subscriber
+        self.subscription = self.create_subscription(
+            String,
+            "robot_description_semantic",
+            self.subscriber_callback,
+            qos_profile=QoSProfile(
+                reliability=QoSReliabilityPolicy.RELIABLE,
+                durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
+                depth=1,
+            ),
+        )
+
+        # Add a spin thread
+        self.ros_spin_thread = Thread(
+            target=lambda node: rclpy.spin(node), args=(self,)
+        )
+        self.ros_spin_thread.start()
+
+    def subscriber_callback(self, msg):
+        self.srdf = msg.data
+        self.msg_event_object.set()
+
+
+# These tests will run concurrently with the dut process.  After all these tests are done,
+# the launch system will shut down the processes that it started up
+class TestFixture(unittest.TestCase):
+    def test_rosout_msgs_published(self, proc_output):
+        rclpy.init()
+        try:
+            node = SrdfObserverNode()
+            node.start_subscriber()
+            msgs_received_flag = node.msg_event_object.wait(timeout=5.0)
+            assert (
+                msgs_received_flag
+            ), "Did not receive message on `/robot_description_semantic`!"
+            assert node.srdf == "test value"
+        finally:
+            rclpy.shutdown()
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        # Check that all processes in the launch (in this case, there's just one) exit
+        # with code 0
+        launch_testing.asserts.assertExitCodes(proc_info)