From 0c69686106b3abdab860530a11ba1af878193dfd Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 12 Feb 2024 15:43:39 -0700 Subject: [PATCH] srdf_publisher_node --- moveit_ros/planning/CMakeLists.txt | 10 ++ moveit_ros/planning/package.xml | 2 + .../srdf_publisher_node/CMakeLists.txt | 14 ++ .../src/srdf_publisher_node.cpp | 82 ++++++++++++ .../test/srdf_publisher_test.py | 125 ++++++++++++++++++ 5 files changed, 233 insertions(+) create mode 100644 moveit_ros/planning/srdf_publisher_node/CMakeLists.txt create mode 100644 moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp create mode 100644 moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index f00ed2f904..3e96154f5b 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/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) diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index b522e4652d..24fe5aae4c 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -34,8 +34,10 @@ moveit_msgs moveit_ros_occupancy_map_monitor rclcpp_action + rclcpp_components rclcpp srdfdom + std_msgs tf2_eigen tf2_geometry_msgs tf2_msgs diff --git a/moveit_ros/planning/srdf_publisher_node/CMakeLists.txt b/moveit_ros/planning/srdf_publisher_node/CMakeLists.txt new file mode 100644 index 0000000000..6ca04e8a75 --- /dev/null +++ b/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() diff --git a/moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp b/moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp new file mode 100644 index 0000000000..81f32481bc --- /dev/null +++ b/moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp @@ -0,0 +1,82 @@ +/********************************************************************* + * 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 */ + +#include +#include + +#include + +namespace moveit_ros_planning +{ + +class SrdfPublisher : public rclcpp::Node +{ + rclcpp::Publisher::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("robot_description_semantic", + rclcpp::QoS(1).transient_local().reliable()); + on_set_parameters_handle_ = + this->add_on_set_parameters_callback([this](const std::vector& parameters) { + for (auto const& parameter : parameters) + { + if (parameter.get_name() == "robot_description_semantic") + { + std_msgs::msg::String msg; + msg.data = parameter.get_value(); + 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) diff --git a/moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py b/moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py new file mode 100644 index 0000000000..a64c29ef36 --- /dev/null +++ b/moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py @@ -0,0 +1,125 @@ +# 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 Willow Garage 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(): + # dut: device under test + 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)