Skip to content

Commit 74feda2

Browse files
tylerjwtylerjw
authored
srdf publisher node (#2682)
1 parent fbad1c2 commit 74feda2

File tree

6 files changed

+295
-47
lines changed

6 files changed

+295
-47
lines changed

‎moveit_ros/planning/CMakeLists.txt

+49-38
Original file line numberDiff line numberDiff line change
@@ -6,76 +6,82 @@ find_package(moveit_common REQUIRED)
66
moveit_package()
77

88
find_package(ament_cmake REQUIRED)
9+
find_package(ament_index_cpp REQUIRED)
10+
find_package(Eigen3 REQUIRED)
911
find_package(fmt REQUIRED)
1012
find_package(generate_parameter_library REQUIRED)
13+
find_package(message_filters REQUIRED)
14+
find_package(moveit_core REQUIRED)
1115
find_package(moveit_msgs REQUIRED)
12-
# find_package(moveit_ros_perception REQUIRED)
16+
find_package(moveit_ros_occupancy_map_monitor REQUIRED)
1317
find_package(pluginlib REQUIRED)
1418
find_package(rclcpp REQUIRED)
15-
find_package(message_filters REQUIRED)
19+
find_package(rclcpp_components REQUIRED)
1620
find_package(srdfdom REQUIRED)
17-
find_package(urdf REQUIRED)
21+
find_package(std_msgs REQUIRED)
1822
find_package(tf2 REQUIRED)
19-
find_package(tf2_geometry_msgs REQUIRED)
2023
find_package(tf2_eigen REQUIRED)
24+
find_package(tf2_geometry_msgs REQUIRED)
2125
find_package(tf2_msgs REQUIRED)
2226
find_package(tf2_ros REQUIRED)
23-
find_package(Eigen3 REQUIRED)
24-
find_package(moveit_core REQUIRED)
25-
find_package(ament_index_cpp REQUIRED)
26-
find_package(moveit_ros_occupancy_map_monitor REQUIRED)
27+
find_package(urdf REQUIRED)
28+
# find_package(moveit_ros_perception REQUIRED)
2729

2830
# Finds Boost Components
2931
include(ConfigExtras.cmake)
3032

3133
set(THIS_PACKAGE_INCLUDE_DIRS
32-
rdf_loader/include
33-
kinematics_plugin_loader/include
34-
robot_model_loader/include
34+
collision_plugin_loader/include
3535
constraint_sampler_manager_loader/include
36-
planning_pipeline/include
36+
kinematics_plugin_loader/include
37+
moveit_cpp/include
38+
plan_execution/include
3739
planning_pipeline_interfaces/include
40+
planning_pipeline/include
3841
planning_scene_monitor/include
39-
trajectory_execution_manager/include
40-
plan_execution/include
41-
collision_plugin_loader/include
42-
moveit_cpp/include)
42+
rdf_loader/include
43+
robot_model_loader/include
44+
trajectory_execution_manager/include)
4345

4446
set(THIS_PACKAGE_LIBRARIES
45-
moveit_rdf_loader
46-
moveit_kinematics_plugin_loader
47-
moveit_robot_model_loader
48-
moveit_constraint_sampler_manager_loader
49-
planning_pipeline_parameters
50-
moveit_planning_pipeline
51-
moveit_planning_pipeline_interfaces
52-
moveit_trajectory_execution_manager
53-
moveit_plan_execution
54-
moveit_planning_scene_monitor
55-
moveit_collision_plugin_loader
5647
default_request_adapter_parameters
5748
default_response_adapter_parameters
49+
moveit_collision_plugin_loader
50+
moveit_constraint_sampler_manager_loader
51+
moveit_cpp
5852
moveit_default_planning_request_adapter_plugins
5953
moveit_default_planning_response_adapter_plugins
60-
moveit_cpp)
54+
moveit_kinematics_plugin_loader
55+
moveit_plan_execution
56+
moveit_planning_pipeline
57+
moveit_planning_pipeline_interfaces
58+
moveit_planning_scene_monitor
59+
moveit_rdf_loader
60+
moveit_robot_model_loader
61+
moveit_trajectory_execution_manager
62+
planning_pipeline_parameters
63+
srdf_publisher_node)
6164

6265
set(THIS_PACKAGE_INCLUDE_DEPENDS
63-
pluginlib
66+
Eigen3
6467
generate_parameter_library
65-
rclcpp
6668
message_filters
69+
moveit_core
70+
moveit_msgs
71+
moveit_ros_occupancy_map_monitor
72+
pluginlib
73+
rclcpp
74+
rclcpp_components
6775
srdfdom
68-
urdf
76+
std_msgs
6977
tf2
7078
tf2_eigen
79+
tf2_geometry_msgs
80+
tf2_msgs
7181
tf2_ros
72-
Eigen3
73-
moveit_core
82+
urdf
7483
# moveit_ros_perception
75-
moveit_ros_occupancy_map_monitor
76-
moveit_msgs
77-
tf2_msgs
78-
tf2_geometry_msgs)
84+
)
7985

8086
include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
8187
include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})
@@ -86,13 +92,14 @@ add_subdirectory(kinematics_plugin_loader)
8692
add_subdirectory(moveit_cpp)
8793
add_subdirectory(plan_execution)
8894
add_subdirectory(planning_components_tools)
89-
add_subdirectory(planning_pipeline)
9095
add_subdirectory(planning_pipeline_interfaces)
96+
add_subdirectory(planning_pipeline)
9197
add_subdirectory(planning_request_adapter_plugins)
9298
add_subdirectory(planning_response_adapter_plugins)
9399
add_subdirectory(planning_scene_monitor)
94100
add_subdirectory(rdf_loader)
95101
add_subdirectory(robot_model_loader)
102+
add_subdirectory(srdf_publisher_node)
96103
add_subdirectory(trajectory_execution_manager)
97104

98105
install(
@@ -107,6 +114,10 @@ install(
107114
ament_export_targets(moveit_ros_planningTargets HAS_LIBRARY_TARGET)
108115
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
109116

117+
rclcpp_components_register_node(
118+
srdf_publisher_node PLUGIN "moveit_ros_planning::SrdfPublisher" EXECUTABLE
119+
srdf_publisher)
120+
110121
pluginlib_export_plugin_description_file(
111122
moveit_core "default_request_adapters_plugin_description.xml")
112123
pluginlib_export_plugin_description_file(

‎moveit_ros/planning/package.xml

+10-7
Original file line numberDiff line numberDiff line change
@@ -24,28 +24,31 @@
2424

2525
<buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
2626

27+
<depend version_gte="1.11.2">pluginlib</depend>
2728
<depend>ament_index_cpp</depend>
29+
<depend>eigen</depend>
30+
<depend>fmt</depend>
2831
<depend>generate_parameter_library</depend>
32+
<depend>message_filters</depend>
2933
<depend>moveit_core</depend>
30-
<depend>moveit_ros_occupancy_map_monitor</depend>
3134
<depend>moveit_msgs</depend>
32-
<depend>message_filters</depend>
33-
<depend version_gte="1.11.2">pluginlib</depend>
35+
<depend>moveit_ros_occupancy_map_monitor</depend>
3436
<depend>rclcpp_action</depend>
37+
<depend>rclcpp_components</depend>
3538
<depend>rclcpp</depend>
3639
<depend>srdfdom</depend>
37-
<depend>urdf</depend>
38-
<depend>tf2</depend>
40+
<depend>std_msgs</depend>
3941
<depend>tf2_eigen</depend>
4042
<depend>tf2_geometry_msgs</depend>
4143
<depend>tf2_msgs</depend>
4244
<depend>tf2_ros</depend>
43-
<depend>eigen</depend>
44-
<depend>fmt</depend>
45+
<depend>tf2</depend>
46+
<depend>urdf</depend>
4547

4648
<test_depend>ament_cmake_gmock</test_depend>
4749
<test_depend>ament_cmake_gtest</test_depend>
4850
<test_depend>ros_testing</test_depend>
51+
<test_depend>launch_testing_ament_cmake</test_depend>
4952

5053
<test_depend>moveit_resources_panda_moveit_config</test_depend>
5154

‎moveit_ros/planning/planning_components_tools/CMakeLists.txt

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
1-
# NOTE: For some reason ament_lint_cmake [linelength] only complains about this
2-
# file
1+
# NOTE: For some reason ament_lint_cmake [linelength] only caware omplains about
2+
# this file
33
# TODO(henningkayser): Disable linelength filter once number of digits is
44
# configurable - set to 120 lint_cmake: -linelength
55
add_executable(moveit_print_planning_model_info

‎moveit_ros/planning/srdf_publisher_node/CMakeLists.txt

+9
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
add_library(srdf_publisher_node SHARED src/srdf_publisher_node.cpp)
2+
ament_target_dependencies(srdf_publisher_node PUBLIC std_msgs rclcpp
3+
rclcpp_components)
4+
5+
if(BUILD_TESTING)
6+
find_package(launch_testing_ament_cmake REQUIRED)
7+
8+
add_launch_test(test/srdf_publisher_test.py TARGET test-srdf_publisher)
9+
endif()

‎moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp

+101
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
/*********************************************************************
2+
* Software License Agreement (BSD License)
3+
*
4+
* Copyright (c) 2024, PickNik Robotics Inc.
5+
* All rights reserved.
6+
*
7+
* Redistribution and use in source and binary forms, with or without
8+
* modification, are permitted provided that the following conditions
9+
* are met:
10+
*
11+
* * Redistributions of source code must retain the above copyright
12+
* notice, this list of conditions and the following disclaimer.
13+
* * Redistributions in binary form must reproduce the above
14+
* copyright notice, this list of conditions and the following
15+
* disclaimer in the documentation and/or other materials provided
16+
* with the distribution.
17+
* * Neither the name of PickNik Robotics Inc. nor the names of its
18+
* contributors may be used to endorse or promote products derived
19+
* from this software without specific prior written permission.
20+
*
21+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28+
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29+
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32+
* POSSIBILITY OF SUCH DAMAGE.
33+
*********************************************************************/
34+
35+
// SrdfPublisher Node
36+
// Author: Tyler Weaver
37+
//
38+
// Node with a single parameter and single publisher using transient local QoS.
39+
// Publishes string set on parameter `robot_description_semantic` to topic
40+
// `/robot_description_semantic`.
41+
//
42+
// This is similar to what [robot_state_publisher](https://github.com/ros/robot_state_publisher)
43+
// does for the URDF using parameter `robot_description` and topic `/robot_description`.
44+
//
45+
// As MoveIt subscribes to the topic `/robot_description_semantic` for the srdf
46+
// this node can be used when you want to set the SRDF parameter on only one node
47+
// and have the rest of your system use a subscriber to that topic to get the
48+
// SRDF.
49+
50+
#include <rclcpp/rclcpp.hpp>
51+
#include <std_msgs/msg/string.hpp>
52+
53+
#include <string>
54+
55+
namespace moveit_ros_planning
56+
{
57+
58+
class SrdfPublisher : public rclcpp::Node
59+
{
60+
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr srdf_publisher_;
61+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_handle_;
62+
63+
public:
64+
SrdfPublisher(const rclcpp::NodeOptions& options) : rclcpp::Node("srdf_publisher", options)
65+
{
66+
srdf_publisher_ = this->create_publisher<std_msgs::msg::String>("robot_description_semantic",
67+
rclcpp::QoS(1).transient_local().reliable());
68+
69+
// TODO: Update the callback used here once Humble is EOL
70+
// Using add_on_set_parameters_callback as it is the only parameter callback available in Humble.
71+
// This is also why we have to return an always success validation.
72+
// Once Humble is EOL use add_post_set_parameters_callback.
73+
on_set_parameters_handle_ =
74+
this->add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter>& parameters) {
75+
for (auto const& parameter : parameters)
76+
{
77+
if (parameter.get_name() == "robot_description_semantic")
78+
{
79+
std_msgs::msg::String msg;
80+
msg.data = parameter.get_value<std::string>();
81+
srdf_publisher_->publish(msg);
82+
break;
83+
}
84+
}
85+
86+
rcl_interfaces::msg::SetParametersResult result;
87+
result.successful = true;
88+
return result;
89+
});
90+
91+
rcl_interfaces::msg::ParameterDescriptor descriptor;
92+
descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
93+
descriptor.description = "Semantic Robot Description Format";
94+
this->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING, descriptor);
95+
}
96+
};
97+
98+
} // namespace moveit_ros_planning
99+
100+
#include "rclcpp_components/register_node_macro.hpp"
101+
RCLCPP_COMPONENTS_REGISTER_NODE(moveit_ros_planning::SrdfPublisher)

0 commit comments

Comments
 (0)