From ccdeb28e93937ddbe47cd9cfa316810c3196edee Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Tue, 17 Sep 2024 18:22:40 +0200 Subject: [PATCH 01/16] Create pose_broadcaster skeleton --- pose_broadcaster/CMakeLists.txt | 72 +++++++++++++++++++ .../pose_broadcaster/pose_broadcaster.hpp | 52 ++++++++++++++ .../pose_broadcaster/visibility_control.h | 49 +++++++++++++ pose_broadcaster/package.xml | 25 +++++++ pose_broadcaster/pose_broadcaster.xml | 9 +++ pose_broadcaster/src/pose_broadcaster.cpp | 63 ++++++++++++++++ 6 files changed, 270 insertions(+) create mode 100644 pose_broadcaster/CMakeLists.txt create mode 100644 pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp create mode 100644 pose_broadcaster/include/pose_broadcaster/visibility_control.h create mode 100644 pose_broadcaster/package.xml create mode 100644 pose_broadcaster/pose_broadcaster.xml create mode 100644 pose_broadcaster/src/pose_broadcaster.cpp diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt new file mode 100644 index 0000000000..190eb76694 --- /dev/null +++ b/pose_broadcaster/CMakeLists.txt @@ -0,0 +1,72 @@ +cmake_minimum_required(VERSION 3.16) +project(pose_broadcaster + LANGUAGES + CXX +) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + generate_parameter_library + geometry_msgs + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +add_library(pose_broadcaster SHARED + src/pose_broadcaster.cpp +) +target_compile_features(pose_broadcaster PUBLIC + cxx_std_17 +) +target_include_directories(pose_broadcaster PUBLIC + $ + $ +) +ament_target_dependencies(pose_broadcaster PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pose_broadcaster PRIVATE "POSE_BROADCASTER_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface pose_broadcaster.xml +) + +if(BUILD_TESTING) + # find_package(ament_lint_auto REQUIRED) + # ament_lint_auto_find_test_dependencies() +endif() + +install( + DIRECTORY + include/ + DESTINATION include/${PROJECT_NAME} +) +install( + TARGETS + pose_broadcaster + EXPORT export_${PROJECT_NAME} + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp new file mode 100644 index 0000000000..c04a572010 --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef POSE_BROADCASTER__POSE_BROADCASTER_HPP_ +#define POSE_BROADCASTER__POSE_BROADCASTER_HPP_ + +#include "controller_interface/controller_interface.hpp" +#include "pose_broadcaster/visibility_control.h" +#include "rclcpp_lifecycle/state.hpp" + +namespace pose_broadcaster +{ + +class PoseBroadcaster : public controller_interface::ControllerInterface +{ +public: + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + POSE_BROADCASTER_PUBLIC controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: +}; + +} // namespace pose_broadcaster + +#endif // POSE_BROADCASTER__POSE_BROADCASTER_HPP_ diff --git a/pose_broadcaster/include/pose_broadcaster/visibility_control.h b/pose_broadcaster/include/pose_broadcaster/visibility_control.h new file mode 100644 index 0000000000..5ce272658d --- /dev/null +++ b/pose_broadcaster/include/pose_broadcaster/visibility_control.h @@ -0,0 +1,49 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_BROADCASTER__VISIBILITY_CONTROL_H_ +#define POSE_BROADCASTER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define POSE_BROADCASTER_EXPORT __attribute__((dllexport)) +#define POSE_BROADCASTER_IMPORT __attribute__((dllimport)) +#else +#define POSE_BROADCASTER_EXPORT __declspec(dllexport) +#define POSE_BROADCASTER_IMPORT __declspec(dllimport) +#endif +#ifdef POSE_BROADCASTER_BUILDING_DLL +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_EXPORT +#else +#define POSE_BROADCASTER_PUBLIC POSE_BROADCASTER_IMPORT +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#else +#define POSE_BROADCASTER_EXPORT __attribute__((visibility("default"))) +#define POSE_BROADCASTER_IMPORT +#if __GNUC__ >= 4 +#define POSE_BROADCASTER_PUBLIC __attribute__((visibility("default"))) +#define POSE_BROADCASTER_LOCAL __attribute__((visibility("hidden"))) +#else +#define POSE_BROADCASTER_PUBLIC +#define POSE_BROADCASTER_LOCAL +#endif +#define POSE_BROADCASTER_PUBLIC_TYPE +#endif + +#endif // POSE_BROADCASTER__VISIBILITY_CONTROL_H_ diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml new file mode 100644 index 0000000000..c0384ad5bb --- /dev/null +++ b/pose_broadcaster/package.xml @@ -0,0 +1,25 @@ + + + + pose_broadcaster + 0.0.0 + Broadcaster to publish cartesian states. + Robert Wilbrandt + + Apache License 2.0 + + ament_cmake + + backward_ros + controller_interface + generate_parameter_library + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + + + ament_cmake + + diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml new file mode 100644 index 0000000000..8247b1e98c --- /dev/null +++ b/pose_broadcaster/pose_broadcaster.xml @@ -0,0 +1,9 @@ + + + + This controller publishes a cartesian state as a geometry_msgs/PoseStamped message. + + + diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp new file mode 100644 index 0000000000..40c7d753d7 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -0,0 +1,63 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "pose_broadcaster/pose_broadcaster.hpp" + +namespace pose_broadcaster +{ + +controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() + const +{ + return controller_interface::InterfaceConfiguration{}; +} + +controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const +{ + return controller_interface::InterfaceConfiguration{}; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_init() +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type PoseBroadcaster::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +} // namespace pose_broadcaster + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(pose_broadcaster::PoseBroadcaster, controller_interface::ControllerInterface) From ea637437b639570d233188ecdd2db91a03c3ba08 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Tue, 17 Sep 2024 20:07:34 +0200 Subject: [PATCH 02/16] Determine state interfaces from parameters --- pose_broadcaster/CMakeLists.txt | 8 +++ .../pose_broadcaster/pose_broadcaster.hpp | 9 +++ pose_broadcaster/src/pose_broadcaster.cpp | 58 ++++++++++++++++++- .../src/pose_broadcaster_parameters.yaml | 39 +++++++++++++ 4 files changed, 113 insertions(+), 1 deletion(-) create mode 100644 pose_broadcaster/src/pose_broadcaster_parameters.yaml diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index 190eb76694..8fefaa4f3c 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -26,6 +26,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library(pose_broadcaster_parameters + src/pose_broadcaster_parameters.yaml +) + add_library(pose_broadcaster SHARED src/pose_broadcaster.cpp ) @@ -36,6 +40,9 @@ target_include_directories(pose_broadcaster PUBLIC $ $ ) +target_link_libraries(pose_broadcaster PUBLIC + pose_broadcaster_parameters +) ament_target_dependencies(pose_broadcaster PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} ) @@ -61,6 +68,7 @@ install( install( TARGETS pose_broadcaster + pose_broadcaster_parameters EXPORT export_${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index c04a572010..6c185e7d86 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -14,8 +14,13 @@ #ifndef POSE_BROADCASTER__POSE_BROADCASTER_HPP_ #define POSE_BROADCASTER__POSE_BROADCASTER_HPP_ +#include +#include +#include + #include "controller_interface/controller_interface.hpp" #include "pose_broadcaster/visibility_control.h" +#include "pose_broadcaster_parameters.hpp" #include "rclcpp_lifecycle/state.hpp" namespace pose_broadcaster @@ -45,6 +50,10 @@ class PoseBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; private: + std::shared_ptr param_listener_; + Params params_; + + std::array interface_names_; }; } // namespace pose_broadcaster diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index 40c7d753d7..28ca9fd020 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -24,17 +24,73 @@ controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_ controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const { - return controller_interface::InterfaceConfiguration{}; + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + std::copy( + interface_names_.cbegin(), interface_names_.cend(), + std::back_inserter(state_interfaces_config.names)); + + return state_interfaces_config; } controller_interface::CallbackReturn PoseBroadcaster::on_init() { + try + { + param_listener_ = std::make_shared(get_node()); + params_ = param_listener_->get_params(); + } + catch (const std::exception & ex) + { + fprintf(stderr, "Exception thrown during init stage with message: %s\n", ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn PoseBroadcaster::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { + params_ = param_listener_->get_params(); + + const bool no_interface_names_defined = + params_.interface_names.position.x.empty() && params_.interface_names.position.y.empty() && + params_.interface_names.position.z.empty() && params_.interface_names.orientation.r.empty() && + params_.interface_names.orientation.p.empty() && params_.interface_names.orientation.y.empty(); + + if (params_.pose_name.empty() && no_interface_names_defined) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'pose_name' or 'interface_names.[position.[x|y|z]|orientation.[r|p|y]]' parameter has to be " + "specified."); + return controller_interface::CallbackReturn::ERROR; + } + + if (!params_.pose_name.empty() && !no_interface_names_defined) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "'pose_name' and 'interface_names.[position.[x|y|z]|orientation.[r|p|y]]' parameters can not " + "be specified together."); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.pose_name.empty()) + { + interface_names_ = { + params_.interface_names.position.x, params_.interface_names.position.y, + params_.interface_names.position.z, params_.interface_names.orientation.r, + params_.interface_names.orientation.p, params_.interface_names.orientation.y}; + } + else + { + interface_names_ = {params_.pose_name + "/position.x", params_.pose_name + "/position.y", + params_.pose_name + "/position.z", params_.pose_name + "/orientation.r", + params_.pose_name + "/orientation.p", params_.pose_name + "/orientation.y"}; + } + return controller_interface::CallbackReturn::SUCCESS; } diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml new file mode 100644 index 0000000000..3ee43b37b4 --- /dev/null +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -0,0 +1,39 @@ +pose_broadcaster: + frame_id: + type: string + default_value: "" + description: "frame_id in which values are published" + validation: + not_empty<>: null + pose_name: + type: string + default_value: "" + description: "Name of the published pose used as prefix for interfaces if there are no individual interface names defined. + If used, standard interface names will be used: ``/position.x, ..., /orientation.y``" + interface_names: + position: + x: + type: string + default_value: "" + description: "Name of the state interface with position values along the 'x' axis" + y: + type: string + default_value: "" + description: "Name of the state interface with position values along the 'y' axis" + z: + type: string + default_value: "" + description: "Name of the state interface with position values along the 'z' axis" + orientation: + r: + type: string + default_value: "" + description: "Name of the state interface with roll values around 'x' axis" + p: + type: string + default_value: "" + description: "Name of the state interface with pitch values around 'y' axis" + y: + type: string + default_value: "" + description: "Name of the state interface with yaw values around 'z' axis" From ac1f29ea4117e398ce2450eab6258b1b79fc89e0 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Tue, 17 Sep 2024 21:04:11 +0200 Subject: [PATCH 03/16] Convert and publish pose --- .../pose_broadcaster/pose_broadcaster.hpp | 10 ++++ pose_broadcaster/src/pose_broadcaster.cpp | 51 ++++++++++++++++++- 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 6c185e7d86..8741b377dc 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -19,9 +19,13 @@ #include #include "controller_interface/controller_interface.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" #include "pose_broadcaster/visibility_control.h" #include "pose_broadcaster_parameters.hpp" +#include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_publisher.h" namespace pose_broadcaster { @@ -50,10 +54,16 @@ class PoseBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; private: + void setRPY(double r, double p, double y, geometry_msgs::msg::Quaternion & q) const; + std::shared_ptr param_listener_; Params params_; std::array interface_names_; + + rclcpp::Publisher::SharedPtr pose_publisher_; + std::unique_ptr> + realtime_publisher_; }; } // namespace pose_broadcaster diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index 28ca9fd020..f9fc59419c 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -91,6 +91,26 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( params_.pose_name + "/orientation.p", params_.pose_name + "/orientation.y"}; } + try + { + pose_publisher_ = get_node()->create_publisher( + "~/pose", rclcpp::SystemDefaultsQoS()); + realtime_publisher_ = + std::make_unique>( + pose_publisher_); + } + catch (const std::exception & ex) + { + fprintf( + stderr, "Exception thrown during publisher creation at configure stage with message: %s\n", + ex.what()); + return controller_interface::CallbackReturn::ERROR; + } + + realtime_publisher_->lock(); + realtime_publisher_->msg_.header.frame_id = params_.frame_id; + realtime_publisher_->unlock(); + return controller_interface::CallbackReturn::SUCCESS; } @@ -107,11 +127,40 @@ controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( } controller_interface::return_type PoseBroadcaster::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + if (realtime_publisher_ && realtime_publisher_->trylock()) + { + realtime_publisher_->msg_.header.stamp = time; + realtime_publisher_->msg_.pose.position.x = state_interfaces_[0].get_value(); + realtime_publisher_->msg_.pose.position.y = state_interfaces_[1].get_value(); + realtime_publisher_->msg_.pose.position.z = state_interfaces_[2].get_value(); + + setRPY( + state_interfaces_[3].get_value(), state_interfaces_[4].get_value(), + state_interfaces_[5].get_value(), realtime_publisher_->msg_.pose.orientation); + + realtime_publisher_->unlockAndPublish(); + } + return controller_interface::return_type::OK; } +void PoseBroadcaster::setRPY(double r, double p, double y, geometry_msgs::msg::Quaternion & q) const +{ + const double sr = std::sin(r / 2); + const double cr = std::cos(r / 2); + const double sp = std::sin(p / 2); + const double cp = std::cos(p / 2); + const double sy = std::sin(y / 2); + const double cy = std::cos(y / 2); + + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; + q.w = cr * cp * cy + sr * sp * sy; +} + } // namespace pose_broadcaster #include "pluginlib/class_list_macros.hpp" From 3c3d28641fee093e409da30b1f3ac85bc71acb89 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Thu, 19 Sep 2024 20:17:06 +0200 Subject: [PATCH 04/16] Don't claim command interfaces --- pose_broadcaster/src/pose_broadcaster.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index f9fc59419c..56ccee2075 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -19,7 +19,9 @@ namespace pose_broadcaster controller_interface::InterfaceConfiguration PoseBroadcaster::command_interface_configuration() const { - return controller_interface::InterfaceConfiguration{}; + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return command_interfaces_config; } controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const From 051073cc22113e0807dc589582b65e0d8413acfd Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Mon, 30 Sep 2024 18:29:51 +0200 Subject: [PATCH 05/16] Use quaternions instead of rpy for orientation representation --- .../pose_broadcaster/pose_broadcaster.hpp | 4 +- pose_broadcaster/src/pose_broadcaster.cpp | 44 ++++++++----------- .../src/pose_broadcaster_parameters.yaml | 18 +++++--- 3 files changed, 30 insertions(+), 36 deletions(-) diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 8741b377dc..72c84fbff7 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -54,12 +54,10 @@ class PoseBroadcaster : public controller_interface::ControllerInterface const rclcpp::Time & time, const rclcpp::Duration & period) override; private: - void setRPY(double r, double p, double y, geometry_msgs::msg::Quaternion & q) const; - std::shared_ptr param_listener_; Params params_; - std::array interface_names_; + std::array interface_names_; rclcpp::Publisher::SharedPtr pose_publisher_; std::unique_ptr> diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index 56ccee2075..613375fed2 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -58,14 +58,16 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( const bool no_interface_names_defined = params_.interface_names.position.x.empty() && params_.interface_names.position.y.empty() && - params_.interface_names.position.z.empty() && params_.interface_names.orientation.r.empty() && - params_.interface_names.orientation.p.empty() && params_.interface_names.orientation.y.empty(); + params_.interface_names.position.z.empty() && params_.interface_names.orientation.x.empty() && + params_.interface_names.orientation.y.empty() && + params_.interface_names.orientation.z.empty() && params_.interface_names.orientation.w.empty(); if (params_.pose_name.empty() && no_interface_names_defined) { RCLCPP_ERROR( get_node()->get_logger(), - "'pose_name' or 'interface_names.[position.[x|y|z]|orientation.[r|p|y]]' parameter has to be " + "'pose_name' or 'interface_names.[position.[x|y|z]|orientation.[x|y|z|w]]' parameter has to " + "be " "specified."); return controller_interface::CallbackReturn::ERROR; } @@ -74,7 +76,8 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( { RCLCPP_ERROR( get_node()->get_logger(), - "'pose_name' and 'interface_names.[position.[x|y|z]|orientation.[r|p|y]]' parameters can not " + "'pose_name' and 'interface_names.[position.[x|y|z]|orientation.[x|y|z|w]]' parameters can " + "not " "be specified together."); return controller_interface::CallbackReturn::ERROR; } @@ -83,14 +86,16 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( { interface_names_ = { params_.interface_names.position.x, params_.interface_names.position.y, - params_.interface_names.position.z, params_.interface_names.orientation.r, - params_.interface_names.orientation.p, params_.interface_names.orientation.y}; + params_.interface_names.position.z, params_.interface_names.orientation.x, + params_.interface_names.orientation.y, params_.interface_names.orientation.z, + params_.interface_names.orientation.w}; } else { interface_names_ = {params_.pose_name + "/position.x", params_.pose_name + "/position.y", - params_.pose_name + "/position.z", params_.pose_name + "/orientation.r", - params_.pose_name + "/orientation.p", params_.pose_name + "/orientation.y"}; + params_.pose_name + "/position.z", params_.pose_name + "/orientation.x", + params_.pose_name + "/orientation.y", params_.pose_name + "/orientation.z", + params_.pose_name + "/orientation.w"}; } try @@ -134,13 +139,15 @@ controller_interface::return_type PoseBroadcaster::update( if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; + realtime_publisher_->msg_.pose.position.x = state_interfaces_[0].get_value(); realtime_publisher_->msg_.pose.position.y = state_interfaces_[1].get_value(); realtime_publisher_->msg_.pose.position.z = state_interfaces_[2].get_value(); - setRPY( - state_interfaces_[3].get_value(), state_interfaces_[4].get_value(), - state_interfaces_[5].get_value(), realtime_publisher_->msg_.pose.orientation); + realtime_publisher_->msg_.pose.orientation.x = state_interfaces_[3].get_value(); + realtime_publisher_->msg_.pose.orientation.y = state_interfaces_[4].get_value(); + realtime_publisher_->msg_.pose.orientation.z = state_interfaces_[5].get_value(); + realtime_publisher_->msg_.pose.orientation.w = state_interfaces_[6].get_value(); realtime_publisher_->unlockAndPublish(); } @@ -148,21 +155,6 @@ controller_interface::return_type PoseBroadcaster::update( return controller_interface::return_type::OK; } -void PoseBroadcaster::setRPY(double r, double p, double y, geometry_msgs::msg::Quaternion & q) const -{ - const double sr = std::sin(r / 2); - const double cr = std::cos(r / 2); - const double sp = std::sin(p / 2); - const double cp = std::cos(p / 2); - const double sy = std::sin(y / 2); - const double cy = std::cos(y / 2); - - q.x = sr * cp * cy - cr * sp * sy; - q.y = cr * sp * cy + sr * cp * sy; - q.z = cr * cp * sy - sr * sp * cy; - q.w = cr * cp * cy + sr * sp * sy; -} - } // namespace pose_broadcaster #include "pluginlib/class_list_macros.hpp" diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml index 3ee43b37b4..ecf82a93d1 100644 --- a/pose_broadcaster/src/pose_broadcaster_parameters.yaml +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -9,7 +9,7 @@ pose_broadcaster: type: string default_value: "" description: "Name of the published pose used as prefix for interfaces if there are no individual interface names defined. - If used, standard interface names will be used: ``/position.x, ..., /orientation.y``" + If used, standard interface names will be used: ``/position.x, ..., /orientation.w``" interface_names: position: x: @@ -25,15 +25,19 @@ pose_broadcaster: default_value: "" description: "Name of the state interface with position values along the 'z' axis" orientation: - r: + x: type: string default_value: "" - description: "Name of the state interface with roll values around 'x' axis" - p: + description: "Name of the state interface with the quaternion orientation x component" + y: type: string default_value: "" - description: "Name of the state interface with pitch values around 'y' axis" - y: + description: "Name of the state interface with the quaternion orientation y component" + z: + type: string + default_value: "" + description: "Name of the state interface with the quaternion orientation z component" + w: type: string default_value: "" - description: "Name of the state interface with yaw values around 'z' axis" + description: "Name of the state interface with the quaternion orientation w component" From 32d39dc49c46f0a5aea8c5eca9b9be20c0ad8213 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Mon, 30 Sep 2024 19:46:50 +0200 Subject: [PATCH 06/16] Add controller loading test --- pose_broadcaster/CMakeLists.txt | 17 ++++++- pose_broadcaster/package.xml | 4 ++ .../test/pose_broadcaster_params.yaml | 3 ++ .../test/test_load_pose_broadcaster.cpp | 50 +++++++++++++++++++ 4 files changed, 72 insertions(+), 2 deletions(-) create mode 100644 pose_broadcaster/test/pose_broadcaster_params.yaml create mode 100644 pose_broadcaster/test/test_load_pose_broadcaster.cpp diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index 8fefaa4f3c..c32aaf5c77 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -56,8 +56,21 @@ pluginlib_export_plugin_description_file( ) if(BUILD_TESTING) - # find_package(ament_lint_auto REQUIRED) - # ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") + ament_add_gmock(test_load_pose_broadcaster + test/test_load_pose_broadcaster.cpp + ) + target_link_libraries(test_load_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_load_pose_broadcaster + controller_manager + ros2_control_test_assets + ) endif() install( diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index c0384ad5bb..29cc793a9f 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -19,6 +19,10 @@ rclcpp_lifecycle realtime_tools + ament_cmake_gmock + controller_manager + ros2_control_test_assets + ament_cmake diff --git a/pose_broadcaster/test/pose_broadcaster_params.yaml b/pose_broadcaster/test/pose_broadcaster_params.yaml new file mode 100644 index 0000000000..74a64f478c --- /dev/null +++ b/pose_broadcaster/test/pose_broadcaster_params.yaml @@ -0,0 +1,3 @@ +test_pose_broadcaster: + ros__parameters: + frame_id: "pose_frame" diff --git a/pose_broadcaster/test/test_load_pose_broadcaster.cpp b/pose_broadcaster/test/test_load_pose_broadcaster.cpp new file mode 100644 index 0000000000..bdf72d7b23 --- /dev/null +++ b/pose_broadcaster/test/test_load_pose_broadcaster.cpp @@ -0,0 +1,50 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadPoseBroadcaster, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm{ + executor, ros2_control_test_assets::minimal_robot_urdf, true, "test_controller_manager"}; + + const std::string test_file_path = + std::string{TEST_FILES_DIRECTORY} + "/pose_broadcaster_params.yaml"; + cm.set_parameter({"test_pose_broadcaster.params_file", test_file_path}); + + cm.set_parameter({"test_pose_broadcaster.type", "pose_broadcaster/PoseBroadcaster"}); + + ASSERT_NE(cm.load_controller("test_pose_broadcaster"), nullptr); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} From cff76acc6ae9a660764dff93b4505a7fbc974df4 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Sat, 5 Oct 2024 12:43:58 +0200 Subject: [PATCH 07/16] Use PoseSensor semantic component --- .../pose_broadcaster/pose_broadcaster.hpp | 4 +- pose_broadcaster/src/pose_broadcaster.cpp | 59 ++----------------- .../src/pose_broadcaster_parameters.yaml | 38 ++---------- .../test/pose_broadcaster_params.yaml | 1 + 4 files changed, 13 insertions(+), 89 deletions(-) diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index 72c84fbff7..cbe91e22ce 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -20,12 +20,12 @@ #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/quaternion.hpp" #include "pose_broadcaster/visibility_control.h" #include "pose_broadcaster_parameters.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" +#include "semantic_components/pose_sensor.hpp" namespace pose_broadcaster { @@ -57,7 +57,7 @@ class PoseBroadcaster : public controller_interface::ControllerInterface std::shared_ptr param_listener_; Params params_; - std::array interface_names_; + std::unique_ptr pose_sensor_; rclcpp::Publisher::SharedPtr pose_publisher_; std::unique_ptr> diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index 613375fed2..bb43c91a3b 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -28,9 +28,7 @@ controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_co { controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - std::copy( - interface_names_.cbegin(), interface_names_.cend(), - std::back_inserter(state_interfaces_config.names)); + state_interfaces_config.names = pose_sensor_->get_state_interface_names(); return state_interfaces_config; } @@ -56,47 +54,7 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( { params_ = param_listener_->get_params(); - const bool no_interface_names_defined = - params_.interface_names.position.x.empty() && params_.interface_names.position.y.empty() && - params_.interface_names.position.z.empty() && params_.interface_names.orientation.x.empty() && - params_.interface_names.orientation.y.empty() && - params_.interface_names.orientation.z.empty() && params_.interface_names.orientation.w.empty(); - - if (params_.pose_name.empty() && no_interface_names_defined) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "'pose_name' or 'interface_names.[position.[x|y|z]|orientation.[x|y|z|w]]' parameter has to " - "be " - "specified."); - return controller_interface::CallbackReturn::ERROR; - } - - if (!params_.pose_name.empty() && !no_interface_names_defined) - { - RCLCPP_ERROR( - get_node()->get_logger(), - "'pose_name' and 'interface_names.[position.[x|y|z]|orientation.[x|y|z|w]]' parameters can " - "not " - "be specified together."); - return controller_interface::CallbackReturn::ERROR; - } - - if (params_.pose_name.empty()) - { - interface_names_ = { - params_.interface_names.position.x, params_.interface_names.position.y, - params_.interface_names.position.z, params_.interface_names.orientation.x, - params_.interface_names.orientation.y, params_.interface_names.orientation.z, - params_.interface_names.orientation.w}; - } - else - { - interface_names_ = {params_.pose_name + "/position.x", params_.pose_name + "/position.y", - params_.pose_name + "/position.z", params_.pose_name + "/orientation.x", - params_.pose_name + "/orientation.y", params_.pose_name + "/orientation.z", - params_.pose_name + "/orientation.w"}; - } + pose_sensor_ = std::make_unique(params_.pose_name); try { @@ -124,12 +82,14 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( controller_interface::CallbackReturn PoseBroadcaster::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + pose_sensor_->assign_loaned_state_interfaces(state_interfaces_); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { + pose_sensor_->release_interfaces(); return controller_interface::CallbackReturn::SUCCESS; } @@ -139,16 +99,7 @@ controller_interface::return_type PoseBroadcaster::update( if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; - - realtime_publisher_->msg_.pose.position.x = state_interfaces_[0].get_value(); - realtime_publisher_->msg_.pose.position.y = state_interfaces_[1].get_value(); - realtime_publisher_->msg_.pose.position.z = state_interfaces_[2].get_value(); - - realtime_publisher_->msg_.pose.orientation.x = state_interfaces_[3].get_value(); - realtime_publisher_->msg_.pose.orientation.y = state_interfaces_[4].get_value(); - realtime_publisher_->msg_.pose.orientation.z = state_interfaces_[5].get_value(); - realtime_publisher_->msg_.pose.orientation.w = state_interfaces_[6].get_value(); - + pose_sensor_->get_values_as_message(realtime_publisher_->msg_.pose); realtime_publisher_->unlockAndPublish(); } diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml index ecf82a93d1..01f0ad5bc8 100644 --- a/pose_broadcaster/src/pose_broadcaster_parameters.yaml +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -8,36 +8,8 @@ pose_broadcaster: pose_name: type: string default_value: "" - description: "Name of the published pose used as prefix for interfaces if there are no individual interface names defined. - If used, standard interface names will be used: ``/position.x, ..., /orientation.w``" - interface_names: - position: - x: - type: string - default_value: "" - description: "Name of the state interface with position values along the 'x' axis" - y: - type: string - default_value: "" - description: "Name of the state interface with position values along the 'y' axis" - z: - type: string - default_value: "" - description: "Name of the state interface with position values along the 'z' axis" - orientation: - x: - type: string - default_value: "" - description: "Name of the state interface with the quaternion orientation x component" - y: - type: string - default_value: "" - description: "Name of the state interface with the quaternion orientation y component" - z: - type: string - default_value: "" - description: "Name of the state interface with the quaternion orientation z component" - w: - type: string - default_value: "" - description: "Name of the state interface with the quaternion orientation w component" + description: "Base name used as prefix for controller interfaces. + The state interface names are: ``/position.x, ..., /position.z, + /orientation.x, ..., /orientation.w``" + validation: + not_empty<>: null diff --git a/pose_broadcaster/test/pose_broadcaster_params.yaml b/pose_broadcaster/test/pose_broadcaster_params.yaml index 74a64f478c..a2f8477dd1 100644 --- a/pose_broadcaster/test/pose_broadcaster_params.yaml +++ b/pose_broadcaster/test/pose_broadcaster_params.yaml @@ -1,3 +1,4 @@ test_pose_broadcaster: ros__parameters: + pose_name: "test_pose" frame_id: "pose_frame" From 524fa3a0f5b636ca83e95012805e85312ca0704e Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Sat, 5 Oct 2024 14:19:55 +0200 Subject: [PATCH 08/16] Implement unit tests --- pose_broadcaster/CMakeLists.txt | 12 + .../test/test_pose_broadcaster.cpp | 215 ++++++++++++++++++ .../test/test_pose_broadcaster.hpp | 58 +++++ 3 files changed, 285 insertions(+) create mode 100644 pose_broadcaster/test/test_pose_broadcaster.cpp create mode 100644 pose_broadcaster/test/test_pose_broadcaster.hpp diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index c32aaf5c77..8ceec8504d 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -58,6 +58,7 @@ pluginlib_export_plugin_description_file( if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) find_package(ros2_control_test_assets REQUIRED) add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test") @@ -71,6 +72,17 @@ if(BUILD_TESTING) controller_manager ros2_control_test_assets ) + + add_rostest_with_parameters_gmock(test_pose_broadcaster + test/test_pose_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/pose_broadcaster_params.yaml + ) + target_link_libraries(test_pose_broadcaster + pose_broadcaster + ) + ament_target_dependencies(test_pose_broadcaster + hardware_interface + ) endif() install( diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp new file mode 100644 index 0000000000..792d6fbe5a --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -0,0 +1,215 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "test_pose_broadcaster.hpp" + +#include +#include + +#include "rclcpp/executors.hpp" + +using hardware_interface::LoanedStateInterface; + +void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } + +void PoseBroadcasterTest::TearDown() { pose_broadcaster_.reset(nullptr); } + +void PoseBroadcasterTest::SetUpPoseBroadcaster() +{ + ASSERT_EQ( + pose_broadcaster_->init( + "test_pose_broadcaster", "", 0, "", pose_broadcaster_->define_custom_node_options()), + controller_interface::return_type::OK); + + std::vector state_interfaces; + state_interfaces.emplace_back(pose_position_x_); + state_interfaces.emplace_back(pose_position_y_); + state_interfaces.emplace_back(pose_position_z_); + state_interfaces.emplace_back(pose_orientation_x_); + state_interfaces.emplace_back(pose_orientation_y_); + state_interfaces.emplace_back(pose_orientation_z_); + state_interfaces.emplace_back(pose_orientation_w_); + + pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); +} + +void PoseBroadcasterTest::subscribe_and_get_message(geometry_msgs::msg::PoseStamped & pose_msg) +{ + // Create node for subscribing + rclcpp::Node node{"test_subscription_node"}; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node.get_node_base_interface()); + + // Create subscription + geometry_msgs::msg::PoseStamped::SharedPtr received_msg; + const auto msg_callback = [&](const geometry_msgs::msg::PoseStamped::SharedPtr msg) + { received_msg = msg; }; + const auto subscription = node.create_subscription( + "/test_pose_broadcaster/pose", 10, msg_callback); + + // Update controller and spin until a message is received + // Since update doesn't guarantee a published message, republish until received + constexpr size_t max_sub_check_loop_count = 5; + for (size_t i = 0; !received_msg; ++i) + { + ASSERT_LT(i, max_sub_check_loop_count); + + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); + + const auto timeout = std::chrono::milliseconds{5}; + const auto until = node.get_clock()->now() + timeout; + while (!received_msg && node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds{10}); + } + } + + pose_msg = *received_msg; +} + +TEST_F(PoseBroadcasterTest, Configure_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command interface configuration + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ(command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + // Verify state interface configuration + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); +} + +TEST_F(PoseBroadcasterTest, Activate_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); + } + + // Deactivate controller + ASSERT_EQ( + pose_broadcaster_->on_deactivate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Verify command and state interface configuration + { + const auto command_interface_conf = pose_broadcaster_->command_interface_configuration(); + EXPECT_EQ( + command_interface_conf.type, controller_interface::interface_configuration_type::NONE); + EXPECT_TRUE(command_interface_conf.names.empty()); + + const auto state_interface_conf = pose_broadcaster_->state_interface_configuration(); + EXPECT_EQ( + state_interface_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + ASSERT_EQ(state_interface_conf.names.size(), 7lu); // Should not change when deactivating + } +} + +TEST_F(PoseBroadcasterTest, Update_Success) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + ASSERT_EQ( + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(PoseBroadcasterTest, PublishSuccess) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Configure and activate controller + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + ASSERT_EQ( + pose_broadcaster_->on_activate(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::SUCCESS); + + // Subscribe to pose topic + geometry_msgs::msg::PoseStamped pose_msg; + subscribe_and_get_message(pose_msg); + + // Verify content of pose message + EXPECT_EQ(pose_msg.header.frame_id, frame_id_); + EXPECT_EQ(pose_msg.pose.position.x, pose_values_[0]); + EXPECT_EQ(pose_msg.pose.position.y, pose_values_[1]); + EXPECT_EQ(pose_msg.pose.position.z, pose_values_[2]); + EXPECT_EQ(pose_msg.pose.orientation.x, pose_values_[3]); + EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); + EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); + EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); +} + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleMock(&argc, argv); + rclcpp::init(argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + + return result; +} diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp new file mode 100644 index 0000000000..4bd510d53f --- /dev/null +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -0,0 +1,58 @@ +// Copyright 2024 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef TEST_POSE_BROADCASTER_HPP_ +#define TEST_POSE_BROADCASTER_HPP_ + +#include + +#include +#include +#include + +#include "pose_broadcaster/pose_broadcaster.hpp" + +using pose_broadcaster::PoseBroadcaster; + +class PoseBroadcasterTest : public ::testing::Test +{ +public: + void SetUp(); + void TearDown(); + + void SetUpPoseBroadcaster(); + +protected: + const std::string pose_name_ = "test_pose"; + const std::string frame_id_ = "pose_frame"; + + std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; + + hardware_interface::StateInterface pose_position_x_{pose_name_, "position.x", &pose_values_[0]}; + hardware_interface::StateInterface pose_position_y_{pose_name_, "position.x", &pose_values_[1]}; + hardware_interface::StateInterface pose_position_z_{pose_name_, "position.x", &pose_values_[2]}; + hardware_interface::StateInterface pose_orientation_x_{ + pose_name_, "orientation.x", &pose_values_[3]}; + hardware_interface::StateInterface pose_orientation_y_{ + pose_name_, "orientation.y", &pose_values_[4]}; + hardware_interface::StateInterface pose_orientation_z_{ + pose_name_, "orientation.z", &pose_values_[5]}; + hardware_interface::StateInterface pose_orientation_w_{ + pose_name_, "orientation.w", &pose_values_[6]}; + + std::unique_ptr pose_broadcaster_; + + void subscribe_and_get_message(geometry_msgs::msg::PoseStamped & pose_msg); +}; + +#endif // TEST_POSE_BROADCASTER_HPP_ From 6c0bb0605619ccd24073cfa74c1b010136efe642 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Sat, 5 Oct 2024 14:28:22 +0200 Subject: [PATCH 09/16] Add documentation --- pose_broadcaster/README.md | 8 ++++++++ pose_broadcaster/doc/userdoc.rst | 27 +++++++++++++++++++++++++++ 2 files changed, 35 insertions(+) create mode 100644 pose_broadcaster/README.md create mode 100644 pose_broadcaster/doc/userdoc.rst diff --git a/pose_broadcaster/README.md b/pose_broadcaster/README.md new file mode 100644 index 0000000000..bf47411048 --- /dev/null +++ b/pose_broadcaster/README.md @@ -0,0 +1,8 @@ +pose_broadcaster +========================================== + +Controller to publish poses provided by pose sensors. + +Pluginlib-Library: pose_broadcaster + +Plugin: pose_broadcaster/PoseBroadcaster (controller_interface::ControllerInterface) diff --git a/pose_broadcaster/doc/userdoc.rst b/pose_broadcaster/doc/userdoc.rst new file mode 100644 index 0000000000..b7f3901a56 --- /dev/null +++ b/pose_broadcaster/doc/userdoc.rst @@ -0,0 +1,27 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pose_broadcaster/doc/userdoc.rst + +.. _pose_broadcaster_userdoc: + +Pose Broadcaster +-------------------------------- +Broadcaster for poses measured by a robot or a sensor. +The published message type is ``geometry_msgs/msg/PoseStamped``. + +The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package). + +Parameters +^^^^^^^^^^^ +This controller uses the `generate_parameter_library `_ to handle its parameters. The parameter `definition file located in the src folder `_ contains descriptions for all the parameters used by the controller. + +List of parameters +========================= +.. generate_parameter_library_details:: ../src/pose_broadcaster_parameters.yaml + + +An example parameter file +========================= + +An example parameter file for this controller can be found in `the test directory `_: + +.. literalinclude:: ../test/pose_broadcaster_params.yaml + :language: yaml From f4bac1a9b68f2ceaa9c64e83e63dcd2c536d9311 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Wed, 9 Oct 2024 09:59:19 +0200 Subject: [PATCH 10/16] Implement TF publishing --- pose_broadcaster/CMakeLists.txt | 1 + .../pose_broadcaster/pose_broadcaster.hpp | 5 ++ pose_broadcaster/package.xml | 1 + pose_broadcaster/src/pose_broadcaster.cpp | 64 +++++++++++++++- .../src/pose_broadcaster_parameters.yaml | 10 +++ .../test/test_pose_broadcaster.cpp | 76 +++++++++---------- .../test/test_pose_broadcaster.hpp | 42 +++++++++- 7 files changed, 157 insertions(+), 42 deletions(-) diff --git a/pose_broadcaster/CMakeLists.txt b/pose_broadcaster/CMakeLists.txt index 8ceec8504d..46028cf258 100644 --- a/pose_broadcaster/CMakeLists.txt +++ b/pose_broadcaster/CMakeLists.txt @@ -18,6 +18,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools + tf2_msgs ) find_package(ament_cmake REQUIRED) diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index cbe91e22ce..be3d716054 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -26,6 +26,7 @@ #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_publisher.h" #include "semantic_components/pose_sensor.hpp" +#include "tf2_msgs/msg/tf_message.hpp" namespace pose_broadcaster { @@ -62,6 +63,10 @@ class PoseBroadcaster : public controller_interface::ControllerInterface rclcpp::Publisher::SharedPtr pose_publisher_; std::unique_ptr> realtime_publisher_; + + rclcpp::Publisher::SharedPtr tf_publisher_; + std::unique_ptr> + realtime_tf_publisher_; }; } // namespace pose_broadcaster diff --git a/pose_broadcaster/package.xml b/pose_broadcaster/package.xml index 29cc793a9f..eae2ccad2c 100644 --- a/pose_broadcaster/package.xml +++ b/pose_broadcaster/package.xml @@ -18,6 +18,7 @@ rclcpp rclcpp_lifecycle realtime_tools + tf2_msgs ament_cmake_gmock controller_manager diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index bb43c91a3b..3182ed38bf 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -13,6 +13,14 @@ // limitations under the License. #include "pose_broadcaster/pose_broadcaster.hpp" +namespace +{ + +constexpr auto DEFAULT_POSE_TOPIC = "~/pose"; +constexpr auto DEFAULT_TF_TOPIC = "/tf"; + +} // namespace + namespace pose_broadcaster { @@ -59,10 +67,28 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( try { pose_publisher_ = get_node()->create_publisher( - "~/pose", rclcpp::SystemDefaultsQoS()); + DEFAULT_POSE_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_publisher_ = std::make_unique>( pose_publisher_); + + if (params_.tf.enable) + { + if (params_.tf.child_frame_id.empty()) + { + fprintf( + stderr, + "Parameter tf.child_frame_id needs to be set to a non-empty value if tf publishing is " + "enabled"); + return controller_interface::CallbackReturn::ERROR; + } + + tf_publisher_ = get_node()->create_publisher( + DEFAULT_TF_TOPIC, rclcpp::SystemDefaultsQoS()); + realtime_tf_publisher_ = + std::make_unique>( + tf_publisher_); + } } catch (const std::exception & ex) { @@ -72,10 +98,24 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( return controller_interface::CallbackReturn::ERROR; } + // Initialize pose message realtime_publisher_->lock(); realtime_publisher_->msg_.header.frame_id = params_.frame_id; realtime_publisher_->unlock(); + // Initialize tf message if tf publishing is enabled + if (realtime_tf_publisher_) + { + realtime_tf_publisher_->lock(); + + realtime_tf_publisher_->msg_.transforms.resize(1); + auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); + tf_transform.header.frame_id = params_.frame_id; + tf_transform.child_frame_id = params_.tf.child_frame_id; + + realtime_tf_publisher_->unlock(); + } + return controller_interface::CallbackReturn::SUCCESS; } @@ -96,13 +136,33 @@ controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( controller_interface::return_type PoseBroadcaster::update( const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { + geometry_msgs::msg::Pose pose; + pose_sensor_->get_values_as_message(pose); + if (realtime_publisher_ && realtime_publisher_->trylock()) { realtime_publisher_->msg_.header.stamp = time; - pose_sensor_->get_values_as_message(realtime_publisher_->msg_.pose); + realtime_publisher_->msg_.pose = pose; realtime_publisher_->unlockAndPublish(); } + if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) + { + auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + tf_transform.header.stamp = time; + + tf_transform.transform.translation.x = pose.position.x; + tf_transform.transform.translation.y = pose.position.y; + tf_transform.transform.translation.z = pose.position.z; + + tf_transform.transform.rotation.x = pose.orientation.x; + tf_transform.transform.rotation.y = pose.orientation.y; + tf_transform.transform.rotation.z = pose.orientation.z; + tf_transform.transform.rotation.w = pose.orientation.w; + + realtime_tf_publisher_->unlockAndPublish(); + } + return controller_interface::return_type::OK; } diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml index 01f0ad5bc8..3117cf4e76 100644 --- a/pose_broadcaster/src/pose_broadcaster_parameters.yaml +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -13,3 +13,13 @@ pose_broadcaster: /orientation.x, ..., /orientation.w``" validation: not_empty<>: null + tf: + enable: + type: bool + default_value: false + description: "Whether to publish the pose as a tf transform" + child_frame_id: + type: string + default_value: "" + description: "Child frame id of published tf transforms. This needs to be set if tf publishing is + enabled." diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index 792d6fbe5a..74c8e2f4a0 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -16,8 +16,6 @@ #include #include -#include "rclcpp/executors.hpp" - using hardware_interface::LoanedStateInterface; void PoseBroadcasterTest::SetUp() { pose_broadcaster_ = std::make_unique(); } @@ -43,41 +41,6 @@ void PoseBroadcasterTest::SetUpPoseBroadcaster() pose_broadcaster_->assign_interfaces({}, std::move(state_interfaces)); } -void PoseBroadcasterTest::subscribe_and_get_message(geometry_msgs::msg::PoseStamped & pose_msg) -{ - // Create node for subscribing - rclcpp::Node node{"test_subscription_node"}; - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node.get_node_base_interface()); - - // Create subscription - geometry_msgs::msg::PoseStamped::SharedPtr received_msg; - const auto msg_callback = [&](const geometry_msgs::msg::PoseStamped::SharedPtr msg) - { received_msg = msg; }; - const auto subscription = node.create_subscription( - "/test_pose_broadcaster/pose", 10, msg_callback); - - // Update controller and spin until a message is received - // Since update doesn't guarantee a published message, republish until received - constexpr size_t max_sub_check_loop_count = 5; - for (size_t i = 0; !received_msg; ++i) - { - ASSERT_LT(i, max_sub_check_loop_count); - - pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); - - const auto timeout = std::chrono::milliseconds{5}; - const auto until = node.get_clock()->now() + timeout; - while (!received_msg && node.get_clock()->now() < until) - { - executor.spin_some(); - std::this_thread::sleep_for(std::chrono::microseconds{10}); - } - } - - pose_msg = *received_msg; -} - TEST_F(PoseBroadcasterTest, Configure_Success) { SetUpPoseBroadcaster(); @@ -103,6 +66,23 @@ TEST_F(PoseBroadcasterTest, Configure_Success) ASSERT_EQ(state_interface_conf.names.size(), 7lu); } +TEST_F(PoseBroadcasterTest, Configure_TF_ChildFrameId_NotSet) +{ + SetUpPoseBroadcaster(); + + // Set 'pose_name' and 'frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); + pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + + // Set 'tf.enable' parameter, but don't set 'tf.child_frame_id' + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + + // Verify that controller cannot be configured + ASSERT_EQ( + pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), + controller_interface::CallbackReturn::ERROR); +} + TEST_F(PoseBroadcasterTest, Activate_Success) { SetUpPoseBroadcaster(); @@ -180,6 +160,10 @@ TEST_F(PoseBroadcasterTest, PublishSuccess) pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); + // Set 'tf.enable' and 'tf.child_frame_id' parameters + pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); + pose_broadcaster_->get_node()->set_parameter({"tf.child_frame_id", tf_child_frame_id_}); + // Configure and activate controller ASSERT_EQ( pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), @@ -190,7 +174,7 @@ TEST_F(PoseBroadcasterTest, PublishSuccess) // Subscribe to pose topic geometry_msgs::msg::PoseStamped pose_msg; - subscribe_and_get_message(pose_msg); + subscribe_and_get_message("/test_pose_broadcaster/pose", pose_msg); // Verify content of pose message EXPECT_EQ(pose_msg.header.frame_id, frame_id_); @@ -201,6 +185,22 @@ TEST_F(PoseBroadcasterTest, PublishSuccess) EXPECT_EQ(pose_msg.pose.orientation.y, pose_values_[4]); EXPECT_EQ(pose_msg.pose.orientation.z, pose_values_[5]); EXPECT_EQ(pose_msg.pose.orientation.w, pose_values_[6]); + + // Subscribe to tf topic + tf2_msgs::msg::TFMessage tf_msg; + subscribe_and_get_message("/tf", tf_msg); + + // Verify content of tf message + ASSERT_EQ(tf_msg.transforms.size(), 1lu); + EXPECT_EQ(tf_msg.transforms[0].header.frame_id, frame_id_); + EXPECT_EQ(tf_msg.transforms[0].child_frame_id, tf_child_frame_id_); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.x, pose_values_[0]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.y, pose_values_[1]); + EXPECT_EQ(tf_msg.transforms[0].transform.translation.z, pose_values_[2]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.x, pose_values_[3]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.y, pose_values_[4]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.z, pose_values_[5]); + EXPECT_EQ(tf_msg.transforms[0].transform.rotation.w, pose_values_[6]); } int main(int argc, char * argv[]) diff --git a/pose_broadcaster/test/test_pose_broadcaster.hpp b/pose_broadcaster/test/test_pose_broadcaster.hpp index 4bd510d53f..a164b2c6ac 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.hpp +++ b/pose_broadcaster/test/test_pose_broadcaster.hpp @@ -20,6 +20,8 @@ #include #include +#include "rclcpp/executors.hpp" + #include "pose_broadcaster/pose_broadcaster.hpp" using pose_broadcaster::PoseBroadcaster; @@ -34,7 +36,8 @@ class PoseBroadcasterTest : public ::testing::Test protected: const std::string pose_name_ = "test_pose"; - const std::string frame_id_ = "pose_frame"; + const std::string frame_id_ = "pose_base_frame"; + const std::string tf_child_frame_id_ = "pose_frame"; std::array pose_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7}; @@ -52,7 +55,42 @@ class PoseBroadcasterTest : public ::testing::Test std::unique_ptr pose_broadcaster_; - void subscribe_and_get_message(geometry_msgs::msg::PoseStamped & pose_msg); + template + void subscribe_and_get_message(const std::string & topic, T & msg); }; +template +void PoseBroadcasterTest::subscribe_and_get_message(const std::string & topic, T & msg) +{ + // Create node for subscribing + rclcpp::Node node{"test_subscription_node"}; + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node.get_node_base_interface()); + + // Create subscription + typename T::SharedPtr received_msg; + const auto msg_callback = [&](const typename T::SharedPtr sub_msg) { received_msg = sub_msg; }; + const auto subscription = node.create_subscription(topic, 10, msg_callback); + + // Update controller and spin until a message is received + // Since update doesn't guarantee a published message, republish until received + constexpr size_t max_sub_check_loop_count = 5; + for (size_t i = 0; !received_msg; ++i) + { + ASSERT_LT(i, max_sub_check_loop_count); + + pose_broadcaster_->update(rclcpp::Time{0}, rclcpp::Duration::from_seconds(0.01)); + + const auto timeout = std::chrono::milliseconds{5}; + const auto until = node.get_clock()->now() + timeout; + while (!received_msg && node.get_clock()->now() < until) + { + executor.spin_some(); + std::this_thread::sleep_for(std::chrono::microseconds{10}); + } + } + + msg = *received_msg; +} + #endif // TEST_POSE_BROADCASTER_HPP_ From 6efebd73498f3a849c7098af6c14bc3fd27f4c84 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Wed, 9 Oct 2024 10:39:38 +0200 Subject: [PATCH 11/16] Default child_frame_id to pose_name --- pose_broadcaster/src/pose_broadcaster.cpp | 18 ++++++++---------- .../src/pose_broadcaster_parameters.yaml | 6 +++--- .../test/test_pose_broadcaster.cpp | 17 ----------------- 3 files changed, 11 insertions(+), 30 deletions(-) diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index 3182ed38bf..ab2652e596 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -74,15 +74,6 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( if (params_.tf.enable) { - if (params_.tf.child_frame_id.empty()) - { - fprintf( - stderr, - "Parameter tf.child_frame_id needs to be set to a non-empty value if tf publishing is " - "enabled"); - return controller_interface::CallbackReturn::ERROR; - } - tf_publisher_ = get_node()->create_publisher( DEFAULT_TF_TOPIC, rclcpp::SystemDefaultsQoS()); realtime_tf_publisher_ = @@ -111,7 +102,14 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( realtime_tf_publisher_->msg_.transforms.resize(1); auto & tf_transform = realtime_tf_publisher_->msg_.transforms.front(); tf_transform.header.frame_id = params_.frame_id; - tf_transform.child_frame_id = params_.tf.child_frame_id; + if (params_.tf.child_frame_id.empty()) + { + tf_transform.child_frame_id = params_.pose_name; + } + else + { + tf_transform.child_frame_id = params_.tf.child_frame_id; + } realtime_tf_publisher_->unlock(); } diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml index 3117cf4e76..9180f68fcd 100644 --- a/pose_broadcaster/src/pose_broadcaster_parameters.yaml +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -16,10 +16,10 @@ pose_broadcaster: tf: enable: type: bool - default_value: false + default_value: true description: "Whether to publish the pose as a tf transform" child_frame_id: type: string default_value: "" - description: "Child frame id of published tf transforms. This needs to be set if tf publishing is - enabled." + description: "Child frame id of published tf transforms. Defaults to ``pose_name`` if left + empty." diff --git a/pose_broadcaster/test/test_pose_broadcaster.cpp b/pose_broadcaster/test/test_pose_broadcaster.cpp index 74c8e2f4a0..0ed2e84619 100644 --- a/pose_broadcaster/test/test_pose_broadcaster.cpp +++ b/pose_broadcaster/test/test_pose_broadcaster.cpp @@ -66,23 +66,6 @@ TEST_F(PoseBroadcasterTest, Configure_Success) ASSERT_EQ(state_interface_conf.names.size(), 7lu); } -TEST_F(PoseBroadcasterTest, Configure_TF_ChildFrameId_NotSet) -{ - SetUpPoseBroadcaster(); - - // Set 'pose_name' and 'frame_id' parameters - pose_broadcaster_->get_node()->set_parameter({"pose_name", pose_name_}); - pose_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_}); - - // Set 'tf.enable' parameter, but don't set 'tf.child_frame_id' - pose_broadcaster_->get_node()->set_parameter({"tf.enable", true}); - - // Verify that controller cannot be configured - ASSERT_EQ( - pose_broadcaster_->on_configure(rclcpp_lifecycle::State{}), - controller_interface::CallbackReturn::ERROR); -} - TEST_F(PoseBroadcasterTest, Activate_Success) { SetUpPoseBroadcaster(); From e7e883590e5692596169ce5b82f5057beaf1d5fd Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Wed, 9 Oct 2024 11:06:17 +0200 Subject: [PATCH 12/16] Implement limiting of tf messages --- .../pose_broadcaster/pose_broadcaster.hpp | 3 ++ pose_broadcaster/src/pose_broadcaster.cpp | 44 ++++++++++++++----- .../src/pose_broadcaster_parameters.yaml | 7 +++ 3 files changed, 44 insertions(+), 10 deletions(-) diff --git a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp index be3d716054..621a90cc85 100644 --- a/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp +++ b/pose_broadcaster/include/pose_broadcaster/pose_broadcaster.hpp @@ -16,6 +16,7 @@ #include #include +#include #include #include "controller_interface/controller_interface.hpp" @@ -64,6 +65,8 @@ class PoseBroadcaster : public controller_interface::ControllerInterface std::unique_ptr> realtime_publisher_; + std::optional tf_publish_period_; + rclcpp::Time tf_last_publish_time_{0, 0, RCL_CLOCK_UNINITIALIZED}; rclcpp::Publisher::SharedPtr tf_publisher_; std::unique_ptr> realtime_tf_publisher_; diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index ab2652e596..e91ba03538 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -63,6 +63,10 @@ controller_interface::CallbackReturn PoseBroadcaster::on_configure( params_ = param_listener_->get_params(); pose_sensor_ = std::make_unique(params_.pose_name); + tf_publish_period_ = + params_.tf.publish_rate == 0.0 + ? std::nullopt + : std::optional{rclcpp::Duration::from_seconds(1.0 / params_.tf.publish_rate)}; try { @@ -146,19 +150,39 @@ controller_interface::return_type PoseBroadcaster::update( if (realtime_tf_publisher_ && realtime_tf_publisher_->trylock()) { - auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; - tf_transform.header.stamp = time; + bool do_publish = false; + // rlcpp::Time comparisons throw if clock types are not the same + if (tf_last_publish_time_.get_clock_type() != time.get_clock_type()) + { + do_publish = true; + } + else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ < time)) + { + do_publish = true; + } + + if (do_publish) + { + auto & tf_transform = realtime_tf_publisher_->msg_.transforms[0]; + tf_transform.header.stamp = time; - tf_transform.transform.translation.x = pose.position.x; - tf_transform.transform.translation.y = pose.position.y; - tf_transform.transform.translation.z = pose.position.z; + tf_transform.transform.translation.x = pose.position.x; + tf_transform.transform.translation.y = pose.position.y; + tf_transform.transform.translation.z = pose.position.z; - tf_transform.transform.rotation.x = pose.orientation.x; - tf_transform.transform.rotation.y = pose.orientation.y; - tf_transform.transform.rotation.z = pose.orientation.z; - tf_transform.transform.rotation.w = pose.orientation.w; + tf_transform.transform.rotation.x = pose.orientation.x; + tf_transform.transform.rotation.y = pose.orientation.y; + tf_transform.transform.rotation.z = pose.orientation.z; + tf_transform.transform.rotation.w = pose.orientation.w; - realtime_tf_publisher_->unlockAndPublish(); + realtime_tf_publisher_->unlockAndPublish(); + + tf_last_publish_time_ = time; + } + else + { + realtime_tf_publisher_->unlock(); + } } return controller_interface::return_type::OK; diff --git a/pose_broadcaster/src/pose_broadcaster_parameters.yaml b/pose_broadcaster/src/pose_broadcaster_parameters.yaml index 9180f68fcd..11c53b5e57 100644 --- a/pose_broadcaster/src/pose_broadcaster_parameters.yaml +++ b/pose_broadcaster/src/pose_broadcaster_parameters.yaml @@ -23,3 +23,10 @@ pose_broadcaster: default_value: "" description: "Child frame id of published tf transforms. Defaults to ``pose_name`` if left empty." + publish_rate: + type: double + default_value: 0.0 + description: "Rate to limit publishing of tf transforms to (Hz). If set to 0, no limiting is + performed." + validation: + gt_eq<>: 0.0 From 00d0ca2c9699e2298aaa2cf437d83ef06933820a Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Wed, 9 Oct 2024 11:22:44 +0200 Subject: [PATCH 13/16] Add pose broadcaster to controller list --- doc/controllers_index.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/controllers_index.rst b/doc/controllers_index.rst index 683f23c202..d5b956aa8a 100644 --- a/doc/controllers_index.rst +++ b/doc/controllers_index.rst @@ -71,6 +71,7 @@ In the sense of ros2_control, broadcasters are still controllers using the same IMU Sensor Broadcaster <../imu_sensor_broadcaster/doc/userdoc.rst> Joint State Broadcaster <../joint_state_broadcaster/doc/userdoc.rst> Range Sensor Broadcaster <../range_sensor_broadcaster/doc/userdoc.rst> + Pose Broadcaster <../pose_broadcaster/doc/userdoc.rst> Common Controller Parameters From b33d422f65338b8337fa3acbbc36793b76139add Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Wed, 9 Oct 2024 12:25:54 +0200 Subject: [PATCH 14/16] Use correct comparison for tf rate limiting Co-authored-by: Felix Exner (fexner) --- pose_broadcaster/src/pose_broadcaster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pose_broadcaster/src/pose_broadcaster.cpp b/pose_broadcaster/src/pose_broadcaster.cpp index e91ba03538..7e3aeaddf3 100644 --- a/pose_broadcaster/src/pose_broadcaster.cpp +++ b/pose_broadcaster/src/pose_broadcaster.cpp @@ -156,7 +156,7 @@ controller_interface::return_type PoseBroadcaster::update( { do_publish = true; } - else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ < time)) + else if (!tf_publish_period_ || (tf_last_publish_time_ + *tf_publish_period_ <= time)) { do_publish = true; } From 316757b4ffe4647d2ec5d66b658683acdfe8ddd7 Mon Sep 17 00:00:00 2001 From: RobertWilbrandt Date: Wed, 9 Oct 2024 12:29:24 +0200 Subject: [PATCH 15/16] Fix typo in controller description Co-authored-by: Felix Exner (fexner) --- pose_broadcaster/pose_broadcaster.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml index 8247b1e98c..022254635e 100644 --- a/pose_broadcaster/pose_broadcaster.xml +++ b/pose_broadcaster/pose_broadcaster.xml @@ -3,7 +3,7 @@ type="pose_broadcaster::PoseBroadcaster" base_class_type="controller_interface::ControllerInterface"> - This controller publishes a cartesian state as a geometry_msgs/PoseStamped message. + This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message. From 18dd3a97b44a4dbe0c92d0824c01dade4109e0d1 Mon Sep 17 00:00:00 2001 From: Robert Wilbrandt Date: Wed, 9 Oct 2024 12:31:39 +0200 Subject: [PATCH 16/16] Note tf publishing in documentation --- pose_broadcaster/doc/userdoc.rst | 2 +- pose_broadcaster/pose_broadcaster.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/pose_broadcaster/doc/userdoc.rst b/pose_broadcaster/doc/userdoc.rst index b7f3901a56..0ae40e2fad 100644 --- a/pose_broadcaster/doc/userdoc.rst +++ b/pose_broadcaster/doc/userdoc.rst @@ -5,7 +5,7 @@ Pose Broadcaster -------------------------------- Broadcaster for poses measured by a robot or a sensor. -The published message type is ``geometry_msgs/msg/PoseStamped``. +Poses are published as ``geometry_msgs/msg/PoseStamped`` messages and optionally as tf transforms. The controller is a wrapper around the ``PoseSensor`` semantic component (see ``controller_interface`` package). diff --git a/pose_broadcaster/pose_broadcaster.xml b/pose_broadcaster/pose_broadcaster.xml index 022254635e..6578958004 100644 --- a/pose_broadcaster/pose_broadcaster.xml +++ b/pose_broadcaster/pose_broadcaster.xml @@ -3,7 +3,7 @@ type="pose_broadcaster::PoseBroadcaster" base_class_type="controller_interface::ControllerInterface"> - This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message. + This controller publishes a Cartesian state as a geometry_msgs/PoseStamped message and optionally as a tf transform.