feat: add TF broadcaster helpers in BaseControllerInterface

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
 

source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp

@@ -7,6 +7,9 @@
 #include <controller_interface/helpers.hpp>
 #include <realtime_tools/realtime_buffer.h>
 #include <realtime_tools/realtime_publisher.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/transform_broadcaster.h>
 
 #include <state_representation/parameters/ParameterMap.hpp>
 
@@ -310,6 +313,40 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
       const std::string& service_name,
       const std::function<ControllerServiceResponse(const std::string& string)>& 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<state_representation::CartesianPose>& 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<state_representation::CartesianPose>& 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<typename T>
+  void publish_transforms(
+      const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& tf_broadcaster,
+      bool is_static = false);
+
   state_representation::ParameterMap parameter_map_;///< ParameterMap for handling parameters
   std::unordered_map<std::string, bool> read_only_parameters_;
   std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
@@ -482,6 +531,12 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
       custom_output_configuration_callables_;
   std::map<std::string, std::function<void(const std::string&, const std::string&)>>
       custom_input_configuration_callables_;
+
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;             ///< TF broadcaster
+  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster_;///< TF static broadcaster
+
+  std::shared_ptr<rclcpp::node_interfaces::NodeParametersInterface> node_parameters_;
+  std::shared_ptr<rclcpp::node_interfaces::NodeTopicsInterface> node_topics_;
 };
 
 template<typename T>
@@ -867,4 +922,30 @@ inline void BaseControllerInterface::write_output(const std::string& name, const
   write_std_output<StringPublishers, std_msgs::msg::String, std::string>(name, data);
 }
 
+template<typename T>
+inline void BaseControllerInterface::publish_transforms(
+    const std::vector<state_representation::CartesianPose>& transforms, const std::shared_ptr<T>& 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<geometry_msgs::msg::TransformStamped> 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

source/modulo_controllers/src/BaseControllerInterface.cpp

@@ -2,6 +2,8 @@
 
 #include <chrono>
 
+#include <console_bridge/console.h>
+
 #include <lifecycle_msgs/msg/state.hpp>
 
 #include <modulo_core/translators/message_readers.hpp>
@@ -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<modulo_interfaces::msg::PredicateCollection>("/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<tf2_ros::TransformBroadcaster>(
+        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<std::allocator<void>> options;
+    this->static_tf_broadcaster_ =
+        std::make_shared<tf2_ros::StaticTransformBroadcaster>(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<state_representation::CartesianPose>{transform});
+}
+
+void BaseControllerInterface::send_transforms(const std::vector<state_representation::CartesianPose>& transforms) {
+  this->publish_transforms(transforms, this->tf_broadcaster_);
+}
+
+void BaseControllerInterface::send_static_transform(const state_representation::CartesianPose& transform) {
+  this->send_static_transforms(std::vector<state_representation::CartesianPose>{transform});
+}
+
+void BaseControllerInterface::send_static_transforms(
+    const std::vector<state_representation::CartesianPose>& transforms) {
+  this->publish_transforms(transforms, this->static_tf_broadcaster_, true);
+}
+
 rclcpp::QoS BaseControllerInterface::get_qos() const {
   return qos_;
 }