From 364fe4803e4c31d61e4d424fcb0128e279337977 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 10:07:14 +0100 Subject: [PATCH] feat: add TF broadcaster helpers in BaseControllerInterface --- CHANGELOG.md | 1 + .../BaseControllerInterface.hpp | 81 +++++++++++++++++++ .../src/BaseControllerInterface.cpp | 46 +++++++++++ 3 files changed, 128 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7945b27d..a6c90afd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,7 @@ Release Versions: - fix(components): remove incorrect log line (#166) - fix(controllers): move predicate publishing rate parameter to BaseControllerInterface (#168) + - 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 bde57816..6946e98b 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -7,6 +7,9 @@ #include #include #include +#include +#include +#include #include @@ -310,6 +313,40 @@ class BaseControllerInterface : public controller_interface::ControllerInterface const std::string& service_name, const std::function& callback); + /** + * @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 @@ -447,6 +484,18 @@ class BaseControllerInterface : public controller_interface::ControllerInterface */ std::string validate_service_name(const std::string& service_name, const std::string& type) const; + /** + * @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_; std::shared_ptr @@ -482,6 +531,12 @@ class BaseControllerInterface : public controller_interface::ControllerInterface custom_output_configuration_callables_; std::map> custom_input_configuration_callables_; + + 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 @@ -867,4 +922,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 b3aaa754..1adc16f1 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -2,6 +2,8 @@ #include +#include + #include #include @@ -40,6 +42,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_); @@ -539,6 +544,47 @@ void BaseControllerInterface::add_service( } } +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_; }