diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 739fddd476..17071ad226 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -6,76 +6,82 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(Eigen3 REQUIRED) find_package(fmt REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(message_filters REQUIRED) +find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) -# find_package(moveit_ros_perception REQUIRED) +find_package(moveit_ros_occupancy_map_monitor REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(message_filters REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(srdfdom REQUIRED) -find_package(urdf REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(moveit_core REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(moveit_ros_occupancy_map_monitor REQUIRED) +find_package(urdf REQUIRED) +# find_package(moveit_ros_perception REQUIRED) # Finds Boost Components include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DIRS - rdf_loader/include - kinematics_plugin_loader/include - robot_model_loader/include + collision_plugin_loader/include constraint_sampler_manager_loader/include - planning_pipeline/include + kinematics_plugin_loader/include + moveit_cpp/include + plan_execution/include planning_pipeline_interfaces/include + planning_pipeline/include planning_scene_monitor/include - trajectory_execution_manager/include - plan_execution/include - collision_plugin_loader/include - moveit_cpp/include) + rdf_loader/include + robot_model_loader/include + trajectory_execution_manager/include) set(THIS_PACKAGE_LIBRARIES - moveit_rdf_loader - moveit_kinematics_plugin_loader - moveit_robot_model_loader - moveit_constraint_sampler_manager_loader - planning_pipeline_parameters - moveit_planning_pipeline - moveit_planning_pipeline_interfaces - moveit_trajectory_execution_manager - moveit_plan_execution - moveit_planning_scene_monitor - moveit_collision_plugin_loader default_request_adapter_parameters default_response_adapter_parameters + moveit_collision_plugin_loader + moveit_constraint_sampler_manager_loader + moveit_cpp moveit_default_planning_request_adapter_plugins moveit_default_planning_response_adapter_plugins - moveit_cpp) + moveit_kinematics_plugin_loader + moveit_plan_execution + moveit_planning_pipeline + moveit_planning_pipeline_interfaces + moveit_planning_scene_monitor + moveit_rdf_loader + moveit_robot_model_loader + moveit_trajectory_execution_manager + planning_pipeline_parameters + srdf_publisher_node) set(THIS_PACKAGE_INCLUDE_DEPENDS - pluginlib + Eigen3 generate_parameter_library - rclcpp message_filters + moveit_core + moveit_msgs + moveit_ros_occupancy_map_monitor + pluginlib + rclcpp + rclcpp_components srdfdom - urdf + std_msgs tf2 tf2_eigen + tf2_geometry_msgs + tf2_msgs tf2_ros - Eigen3 - moveit_core + urdf # moveit_ros_perception - moveit_ros_occupancy_map_monitor - moveit_msgs - tf2_msgs - tf2_geometry_msgs) +) include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) @@ -86,13 +92,14 @@ add_subdirectory(kinematics_plugin_loader) add_subdirectory(moveit_cpp) add_subdirectory(plan_execution) add_subdirectory(planning_components_tools) -add_subdirectory(planning_pipeline) add_subdirectory(planning_pipeline_interfaces) +add_subdirectory(planning_pipeline) add_subdirectory(planning_request_adapter_plugins) 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( @@ -107,6 +114,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( diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index a6085907b7..a343e6f89a 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -24,28 +24,31 @@ eigen3_cmake_module + pluginlib ament_index_cpp + eigen + fmt generate_parameter_library + message_filters moveit_core - moveit_ros_occupancy_map_monitor moveit_msgs - message_filters - pluginlib + moveit_ros_occupancy_map_monitor rclcpp_action + rclcpp_components rclcpp srdfdom - urdf - tf2 + std_msgs tf2_eigen tf2_geometry_msgs tf2_msgs tf2_ros - eigen - fmt + tf2 + urdf ament_cmake_gmock ament_cmake_gtest ros_testing + launch_testing_ament_cmake moveit_resources_panda_moveit_config diff --git a/moveit_ros/planning/planning_components_tools/CMakeLists.txt b/moveit_ros/planning/planning_components_tools/CMakeLists.txt index 808d65feb3..0eb2bd3fab 100644 --- a/moveit_ros/planning/planning_components_tools/CMakeLists.txt +++ b/moveit_ros/planning/planning_components_tools/CMakeLists.txt @@ -1,5 +1,5 @@ -# NOTE: For some reason ament_lint_cmake [linelength] only complains about this -# file +# NOTE: For some reason ament_lint_cmake [linelength] only caware omplains about +# this file # TODO(henningkayser): Disable linelength filter once number of digits is # configurable - set to 120 lint_cmake: -linelength add_executable(moveit_print_planning_model_info 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..d548d2f1e6 --- /dev/null +++ b/moveit_ros/planning/srdf_publisher_node/CMakeLists.txt @@ -0,0 +1,9 @@ +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 REQUIRED) + + 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..6684e39b16 --- /dev/null +++ b/moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp @@ -0,0 +1,101 @@ +/********************************************************************* + * 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 +#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()); + + // TODO: Update the callback used here once Humble is EOL + // Using add_on_set_parameters_callback as it is the only parameter callback available in Humble. + // This is also why we have to return an always success validation. + // Once Humble is EOL use add_post_set_parameters_callback. + 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); + break; + } + } + + 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..88d875bf10 --- /dev/null +++ b/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)