From 1f6b6938cdf3400a90f8ab37383d9698c90d7766 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 10:07:14 +0100 Subject: [PATCH 1/6] feat: add TF broadcaster helpers in BaseControllerInterface --- CHANGELOG.md | 3 +- .../BaseControllerInterface.hpp | 82 ++++++++++++++++++- .../src/BaseControllerInterface.cpp | 44 ++++++++++ 3 files changed, 126 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8d0290ac..285ae46f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,7 +22,8 @@ Release Versions: - fix(components): remove incorrect log line (#166) - fix(controllers): move predicate publishing rate parameter to BaseControllerInterface (#168) - - feat(controllers): add TF listener interface in BaseControllerInterface (#169) + - feat(controllers): add TF listener in BaseControllerInterface (#169) + - feat(controllers): add TF broadcaster in BaseControllerInterface (#170) ## 5.0.2 diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index e442c99f..ed050ca1 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include #include @@ -345,6 +347,40 @@ class BaseControllerInterface : public controller_interface::ControllerInterface const std::string& frame, const std::string& reference_frame = "world", double validity_period = -1.0, const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10))); + /** + *@brief Configure a transform broadcaster. + */ + void add_tf_broadcaster(); + + /** + * @brief Configure a static transform broadcaster. + */ + void add_static_tf_broadcaster(); + + /** + * @brief Send a transform to TF. + * @param transform The transform to send + */ + void send_transform(const state_representation::CartesianPose& transform); + + /** + * @brief Send a vector of transforms to TF. + * @param transforms The vector of transforms to send + */ + void send_transforms(const std::vector& transforms); + + /** + * @brief Send a static transform to TF. + * @param transform The transform to send + */ + void send_static_transform(const state_representation::CartesianPose& transform); + + /** + * @brief Send a vector of static transforms to TF. + * @param transforms The vector of transforms to send + */ + void send_static_transforms(const std::vector& transforms); + /** * @brief Getter of the Quality of Service attribute. * @return The Quality of Service attribute @@ -495,6 +531,17 @@ class BaseControllerInterface : public controller_interface::ControllerInterface [[nodiscard]] geometry_msgs::msg::TransformStamped lookup_ros_transform( const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, const tf2::Duration& duration); + /** + * @brief Helper function to send a vector of transforms through a transform broadcaster + * @tparam T The type of the broadcaster (tf2_ros::TransformBroadcaster or tf2_ros::StaticTransformBroadcaster) + * @param transforms The transforms to send + * @param tf_broadcaster A pointer to a configured transform broadcaster object + * @param is_static If true, treat the broadcaster as a static frame broadcaster for the sake of log messages + */ + template + void publish_transforms( + const std::vector& transforms, const std::shared_ptr& tf_broadcaster, + bool is_static = false); state_representation::ParameterMap parameter_map_;///< ParameterMap for handling parameters std::unordered_map read_only_parameters_; @@ -532,8 +579,13 @@ class BaseControllerInterface : public controller_interface::ControllerInterface std::map> custom_input_configuration_callables_; - std::shared_ptr tf_buffer_; ///< TF buffer - std::shared_ptr tf_listener_;///< TF listener + std::shared_ptr tf_buffer_; ///< TF buffer + std::shared_ptr tf_listener_; ///< TF listener + std::shared_ptr tf_broadcaster_; ///< TF broadcaster + std::shared_ptr static_tf_broadcaster_;///< TF static broadcaster + + std::shared_ptr node_parameters_; + std::shared_ptr node_topics_; }; template @@ -919,4 +971,30 @@ inline void BaseControllerInterface::write_output(const std::string& name, const write_std_output(name, data); } +template +inline void BaseControllerInterface::publish_transforms( + const std::vector& transforms, const std::shared_ptr& tf_broadcaster, + bool is_static) { + std::string modifier = is_static ? "static " : ""; + if (tf_broadcaster == nullptr) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000, + "Failed to send " << modifier << "transform: No " << modifier << "TF broadcaster configured."); + return; + } + try { + std::vector transform_messages; + transform_messages.reserve(transforms.size()); + for (const auto& tf : transforms) { + geometry_msgs::msg::TransformStamped transform_message; + modulo_core::translators::write_message(transform_message, tf, this->get_node()->get_clock()->now()); + transform_messages.emplace_back(transform_message); + } + tf_broadcaster->sendTransform(transform_messages); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_node()->get_logger(), *this->get_node()->get_clock(), 1000, + "Failed to send " << modifier << "transform: " << ex.what()); + } +} }// namespace modulo_controllers diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 4fb91678..0dddfc85 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -43,6 +43,9 @@ BaseControllerInterface::on_configure(const rclcpp_lifecycle::State&) { add_inputs(); add_outputs(); + this->node_parameters_ = get_node()->get_node_parameters_interface(); + this->node_topics_ = get_node()->get_node_topics_interface(); + if (predicates_.size()) { predicate_publisher_ = get_node()->create_publisher("/predicates", qos_); @@ -595,6 +598,47 @@ geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transfo } } +void BaseControllerInterface::add_tf_broadcaster() { + if (this->tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + this->tf_broadcaster_ = std::make_shared( + this->node_parameters_, this->node_topics_, tf2_ros::DynamicBroadcasterQoS()); + } else { + RCLCPP_DEBUG(this->get_node()->get_logger(), "TF broadcaster already exists."); + } +} + +void BaseControllerInterface::add_static_tf_broadcaster() { + if (this->static_tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + tf2_ros::StaticBroadcasterQoS qos; + rclcpp::PublisherOptionsWithAllocator> options; + this->static_tf_broadcaster_ = + std::make_shared(this->node_parameters_, this->node_topics_, qos, options); + } else { + RCLCPP_DEBUG(this->get_node()->get_logger(), "Static TF broadcaster already exists."); + } +} + +void BaseControllerInterface::send_transform(const state_representation::CartesianPose& transform) { + this->send_transforms(std::vector{transform}); +} + +void BaseControllerInterface::send_transforms(const std::vector& transforms) { + this->publish_transforms(transforms, this->tf_broadcaster_); +} + +void BaseControllerInterface::send_static_transform(const state_representation::CartesianPose& transform) { + this->send_static_transforms(std::vector{transform}); +} + +void BaseControllerInterface::send_static_transforms( + const std::vector& transforms) { + this->publish_transforms(transforms, this->static_tf_broadcaster_, true); +} + rclcpp::QoS BaseControllerInterface::get_qos() const { return qos_; } From dcae4e5eb354fc8e4725ce77f2e74d7e2652ac0e Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 10:35:24 +0100 Subject: [PATCH 2/6] fix: correctly init interfaces on init --- source/modulo_controllers/src/BaseControllerInterface.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 0dddfc85..7963d819 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -34,6 +34,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseCo [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { return this->on_set_parameters_callback(parameters); }); + this->node_parameters_ = get_node()->get_node_parameters_interface(); + this->node_topics_ = get_node()->get_node_topics_interface(); add_parameter("predicate_publishing_rate", 10.0, "The rate at which to publish controller predicates"); return CallbackReturn::SUCCESS; } @@ -43,9 +45,6 @@ BaseControllerInterface::on_configure(const rclcpp_lifecycle::State&) { add_inputs(); add_outputs(); - this->node_parameters_ = get_node()->get_node_parameters_interface(); - this->node_topics_ = get_node()->get_node_topics_interface(); - if (predicates_.size()) { predicate_publisher_ = get_node()->create_publisher("/predicates", qos_); From d9c65b6368a1d2e87653c45e1d3f48fe1ec56dc0 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 13:35:43 +0100 Subject: [PATCH 3/6] fix: check if node is initialized before adding TF broadcasters --- source/modulo_controllers/src/BaseControllerInterface.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 7963d819..3dcfdba8 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -598,6 +598,9 @@ geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transfo } void BaseControllerInterface::add_tf_broadcaster() { + if (this->get_node() == nullptr) { + throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet."); + } if (this->tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); @@ -609,6 +612,9 @@ void BaseControllerInterface::add_tf_broadcaster() { } void BaseControllerInterface::add_static_tf_broadcaster() { + if (this->get_node() == nullptr) { + throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet."); + } if (this->static_tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); From fa09bd5b63a4a7a27df986123104f1450b953e81 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 14:18:32 +0100 Subject: [PATCH 4/6] fix: message typos and extra check in publish_transforms --- .../include/modulo_controllers/BaseControllerInterface.hpp | 3 +++ source/modulo_controllers/src/BaseControllerInterface.cpp | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index ed050ca1..ad4646c8 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -975,6 +975,9 @@ template inline void BaseControllerInterface::publish_transforms( const std::vector& transforms, const std::shared_ptr& tf_broadcaster, bool is_static) { + if (this->get_node() == nullptr) { + throw modulo_core::exceptions::CoreException("Failed publish transforms: Node is not initialized yet."); + } std::string modifier = is_static ? "static " : ""; if (tf_broadcaster == nullptr) { RCLCPP_ERROR_STREAM_THROTTLE( diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 3dcfdba8..6ca56c0c 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -599,7 +599,7 @@ geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transfo void BaseControllerInterface::add_tf_broadcaster() { if (this->get_node() == nullptr) { - throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet."); + throw modulo_core::exceptions::CoreException("Failed to add TF broadcaster: Node is not initialized yet."); } if (this->tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster."); @@ -613,7 +613,7 @@ void BaseControllerInterface::add_tf_broadcaster() { void BaseControllerInterface::add_static_tf_broadcaster() { if (this->get_node() == nullptr) { - throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet."); + throw modulo_core::exceptions::CoreException("Failed to add static TF broadcaster: Node is not initialized yet."); } if (this->static_tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster."); From 25f209d5eb25a889c4779d2a9d52049804988c9e Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 14:22:55 +0100 Subject: [PATCH 5/6] refactor: simplify the Broadcaster initialization --- .../modulo_controllers/BaseControllerInterface.hpp | 3 --- .../modulo_controllers/src/BaseControllerInterface.cpp | 10 ++-------- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index ad4646c8..c07fac73 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -583,9 +583,6 @@ class BaseControllerInterface : public controller_interface::ControllerInterface std::shared_ptr tf_listener_; ///< TF listener std::shared_ptr tf_broadcaster_; ///< TF broadcaster std::shared_ptr static_tf_broadcaster_;///< TF static broadcaster - - std::shared_ptr node_parameters_; - std::shared_ptr node_topics_; }; template diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 6ca56c0c..3fc9637f 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -34,8 +34,6 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseCo [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { return this->on_set_parameters_callback(parameters); }); - this->node_parameters_ = get_node()->get_node_parameters_interface(); - this->node_topics_ = get_node()->get_node_topics_interface(); add_parameter("predicate_publishing_rate", 10.0, "The rate at which to publish controller predicates"); return CallbackReturn::SUCCESS; } @@ -604,8 +602,7 @@ void BaseControllerInterface::add_tf_broadcaster() { if (this->tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - this->tf_broadcaster_ = std::make_shared( - this->node_parameters_, this->node_topics_, tf2_ros::DynamicBroadcasterQoS()); + this->tf_broadcaster_ = std::make_shared(get_node()); } else { RCLCPP_DEBUG(this->get_node()->get_logger(), "TF broadcaster already exists."); } @@ -618,10 +615,7 @@ void BaseControllerInterface::add_static_tf_broadcaster() { if (this->static_tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - tf2_ros::StaticBroadcasterQoS qos; - rclcpp::PublisherOptionsWithAllocator> options; - this->static_tf_broadcaster_ = - std::make_shared(this->node_parameters_, this->node_topics_, qos, options); + this->static_tf_broadcaster_ = std::make_shared(get_node()); } else { RCLCPP_DEBUG(this->get_node()->get_logger(), "Static TF broadcaster already exists."); } From 9da7b11fcb3b5581cd576c6bb199d82f3364c5ac Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros Date: Thu, 5 Dec 2024 14:30:16 +0100 Subject: [PATCH 6/6] Update source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com> --- .../include/modulo_controllers/BaseControllerInterface.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index c07fac73..c2e17ec4 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -973,7 +973,7 @@ inline void BaseControllerInterface::publish_transforms( const std::vector& transforms, const std::shared_ptr& tf_broadcaster, bool is_static) { if (this->get_node() == nullptr) { - throw modulo_core::exceptions::CoreException("Failed publish transforms: Node is not initialized yet."); + throw modulo_core::exceptions::CoreException("Failed send transform(s): Node is not initialized yet."); } std::string modifier = is_static ? "static " : ""; if (tf_broadcaster == nullptr) {