feat: add TF broadcaster in BaseControllerInterface

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
 

source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp

@@ -8,6 +8,8 @@
 #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 <tf2_ros/transform_listener.h>
 
 #include <state_representation/parameters/ParameterMap.hpp>
@@ -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<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
@@ -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<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_;
@@ -532,8 +579,13 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
   std::map<std::string, std::function<void(const std::string&, const std::string&)>>
       custom_input_configuration_callables_;
 
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;             ///< TF buffer
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;///< TF listener
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;                                ///< TF buffer
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;                   ///< TF listener
+  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>
@@ -919,4 +971,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

@@ -34,6 +34,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BaseCo
       [this](const std::vector<rclcpp::Parameter>& 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<double>("predicate_publishing_rate", 10.0, "The rate at which to publish controller predicates");
   return CallbackReturn::SUCCESS;
 }
@@ -595,6 +597,53 @@ 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);
+    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->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);
+    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_;
 }