Add TF listener and broadcaster in controller base class

CHANGELOG.md

@@ -29,6 +29,9 @@ Release Versions:
 ## Upcoming changes
 
 - feat(controllers)!: remove robot description parameter (#186)
+- feat(controllers)!: add TF listener interface in BaseControllerInterface (#169)
+- feat(controllers)!: add TF broadcaster in BaseControllerInterface (#170)
+- test(controllers): add TF listener and broadcaster tests (#172)
 
 ## 5.4.1
 

source/modulo_components/src/ComponentInterface.cpp

@@ -538,7 +538,7 @@ geometry_msgs::msg::TransformStamped ComponentInterface::lookup_ros_transform(
     const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
     const tf2::Duration& duration) {
   if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
-    throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured.");
+    throw exceptions::LookupTransformException("Failed to lookup transform: No TF buffer / listener configured.");
   }
   try {
     return this->tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);

source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp

@@ -8,6 +8,10 @@
 #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 <tf2_ros/transform_listener.h>
 
 #include <state_representation/parameters/ParameterMap.hpp>
 
@@ -326,6 +330,73 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
       const std::string& service_name,
       const std::function<ControllerServiceResponse(const std::string& string)>& callback);
 
+  /**
+   * @brief Configure a transform buffer and listener.
+   */
+  void add_tf_listener();
+
+  /**
+   * @brief Look up a transform from TF.
+   * @param frame The desired frame of the transform
+   * @param reference_frame The desired reference frame of the transform
+   * @param time_point The time at which the value of the transform is desired
+   * @param duration How long to block the lookup call before failing
+   * @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured or
+   * if the lookupTransform call failed
+   * @return If it exists, the requested transform
+   */
+  [[nodiscard]] state_representation::CartesianPose lookup_transform(
+      const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
+      const tf2::Duration& duration);
+
+  /**
+   * @brief Look up a transform from TF.
+   * @param frame The desired frame of the transform
+   * @param reference_frame The desired reference frame of the transform
+   * @param validity_period The validity period of the latest transform from the time of lookup in seconds
+   * @param duration How long to block the lookup call before failing
+   * @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured,
+   * if the lookupTransform call failed, or if the transform is too old
+   * @return If it exists and is still valid, the requested transform
+   */
+  [[nodiscard]] state_representation::CartesianPose lookup_transform(
+      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
@@ -476,6 +547,32 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
    */
   std::string validate_service_name(const std::string& service_name, const std::string& type) const;
 
+  /**
+   * @brief Helper method to look up a transform from TF.
+   * @param frame The desired frame of the transform
+   * @param reference_frame The desired reference frame of the transform
+   * @param time_point The time at which the value of the transform is desired
+   * @param duration How long to block the lookup call before failing
+   * @throws modulo_core::exceptions::LookupTransformException if TF buffer/listener are unconfigured or
+   * if the lookupTransform call failed
+   * @return If it exists, the requested transform
+   */
+  [[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_;
   std::shared_ptr<rclcpp::node_interfaces::PreSetParametersCallbackHandle>
@@ -511,6 +608,11 @@ 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::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
 };
 
 template<typename T>
@@ -957,4 +1059,31 @@ BaseControllerInterface::create_service(const std::string& service_name, const s
   }
 }
 
+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

@@ -1,7 +1,10 @@
 #include "modulo_controllers/BaseControllerInterface.hpp"
 
+#include <console_bridge/console.h>
+
 #include <lifecycle_msgs/msg/state.hpp>
 
+#include <modulo_core/exceptions.hpp>
 #include <modulo_core/translators/message_readers.hpp>
 
 template<class... Ts>
@@ -132,7 +135,7 @@ void BaseControllerInterface::pre_set_parameters_callback(std::vector<rclcpp::Pa
 
       // convert the ROS parameter into a ParameterInterface without modifying the original
       auto new_parameter = translators::read_parameter_const(ros_parameter, parameter);
-      if (!this->validate_parameter(new_parameter)) {
+      if (!validate_parameter(new_parameter)) {
         result.successful = false;
         result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!";
       } else if (!new_parameter->is_empty()) {
@@ -191,7 +194,7 @@ void BaseControllerInterface::add_predicate(
     RCLCPP_DEBUG(get_node()->get_logger(), "Adding predicate '%s'.", predicate_name.c_str());
   }
   try {
-    this->predicates_.insert_or_assign(predicate_name, Predicate(predicate_function));
+    predicates_.insert_or_assign(predicate_name, Predicate(predicate_function));
   } catch (const std::exception& ex) {
     RCLCPP_ERROR(
         get_node()->get_logger(), "Failed to evaluate callback of predicate '%s', returning false: %s",
@@ -495,24 +498,110 @@ BaseControllerInterface::validate_service_name(const std::string& service_name,
 
 void BaseControllerInterface::add_service(
     const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback) {
-  this->create_service<true>(service_name, callback);
+  create_service<true>(service_name, callback);
 }
 
 void BaseControllerInterface::add_service(
     const std::string& service_name,
     const std::function<ControllerServiceResponse(const std::string& string)>& callback) {
-  this->create_service<true>(service_name, callback);
+  create_service<true>(service_name, callback);
 }
 
 void BaseControllerInterface::add_service_lockfree(
     const std::string& service_name, const std::function<ControllerServiceResponse(void)>& callback) {
-  this->create_service<false>(service_name, callback);
+  create_service<false>(service_name, callback);
 }
 
 void BaseControllerInterface::add_service_lockfree(
     const std::string& service_name,
     const std::function<ControllerServiceResponse(const std::string& string)>& callback) {
-  this->create_service<false>(service_name, callback);
+  create_service<false>(service_name, callback);
+}
+
+void BaseControllerInterface::add_tf_listener() {
+  if (tf_buffer_ == nullptr || tf_listener_ == nullptr) {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Adding TF buffer and listener.");
+    console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
+    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_node()->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+  } else {
+    RCLCPP_DEBUG(get_node()->get_logger(), "TF buffer and listener already exist.");
+  }
+}
+
+state_representation::CartesianPose BaseControllerInterface::lookup_transform(
+    const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
+    const tf2::Duration& duration) {
+  auto transform = lookup_ros_transform(frame, reference_frame, time_point, duration);
+  state_representation::CartesianPose result(frame, reference_frame);
+  translators::read_message(result, transform);
+  return result;
+}
+
+state_representation::CartesianPose BaseControllerInterface::lookup_transform(
+    const std::string& frame, const std::string& reference_frame, double validity_period,
+    const tf2::Duration& duration) {
+  auto transform =
+      lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration);
+  if (validity_period > 0.0
+      && (get_node()->get_clock()->now() - transform.header.stamp).seconds() > validity_period) {
+    throw modulo_core::exceptions::LookupTransformException("Failed to lookup transform: Latest transform is too old!");
+  }
+  state_representation::CartesianPose result(frame, reference_frame);
+  translators::read_message(result, transform);
+  return result;
+}
+
+geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transform(
+    const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point,
+    const tf2::Duration& duration) {
+  if (tf_buffer_ == nullptr || tf_listener_ == nullptr) {
+    throw modulo_core::exceptions::LookupTransformException(
+        "Failed to lookup transform: No TF buffer / listener configured.");
+  }
+  try {
+    return tf_buffer_->lookupTransform(reference_frame, frame, time_point, duration);
+  } catch (const tf2::TransformException& ex) {
+    throw modulo_core::exceptions::LookupTransformException(
+        std::string("Failed to lookup transform: ").append(ex.what()));
+  }
+}
+
+void BaseControllerInterface::add_tf_broadcaster() {
+  if (tf_broadcaster_ == nullptr) {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Adding TF broadcaster.");
+    console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
+    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(get_node());
+  } else {
+    RCLCPP_DEBUG(get_node()->get_logger(), "TF broadcaster already exists.");
+  }
+}
+
+void BaseControllerInterface::add_static_tf_broadcaster() {
+  if (static_tf_broadcaster_ == nullptr) {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Adding static TF broadcaster.");
+    console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
+    static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(get_node());
+  } else {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Static TF broadcaster already exists.");
+  }
+}
+
+void BaseControllerInterface::send_transform(const state_representation::CartesianPose& transform) {
+  send_transforms(std::vector<state_representation::CartesianPose>{transform});
+}
+
+void BaseControllerInterface::send_transforms(const std::vector<state_representation::CartesianPose>& transforms) {
+  publish_transforms(transforms, tf_broadcaster_);
+}
+
+void BaseControllerInterface::send_static_transform(const state_representation::CartesianPose& transform) {
+  send_static_transforms(std::vector<state_representation::CartesianPose>{transform});
+}
+
+void BaseControllerInterface::send_static_transforms(
+    const std::vector<state_representation::CartesianPose>& transforms) {
+  publish_transforms(transforms, static_tf_broadcaster_, true);
 }
 
 rclcpp::QoS BaseControllerInterface::get_qos() const {