feat(controllers): add TF listener and lookup TF helpers in BaseContr…

…ollerInterface (#169)

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 listener interface in BaseControllerInterface (#169)
 
 ## 5.0.2
 

source/modulo_components/src/ComponentInterface.cpp

@@ -537,7 +537,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

@@ -7,6 +7,8 @@
 #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/transform_listener.h>
 
 #include <state_representation/parameters/ParameterMap.hpp>
 
@@ -310,6 +312,39 @@ 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 Getter of the Quality of Service attribute.
    * @return The Quality of Service attribute
@@ -334,6 +369,12 @@ class BaseControllerInterface : public controller_interface::ControllerInterface
    */
   std::timed_mutex& get_command_mutex();
 
+  /**
+   * @brief Check if the node has been initialized or not.
+   * @return True if the node is initialized, false otherwise
+   */
+  bool is_node_initialized() const;
+
 private:
   /**
    * @brief Parameter validation function
@@ -447,6 +488,20 @@ 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);
+
   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 +537,9 @@ 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
 };
 
 template<typename T>

source/modulo_controllers/src/BaseControllerInterface.cpp

@@ -2,9 +2,13 @@
 
 #include <chrono>
 
+#include <console_bridge/console.h>
+
 #include <lifecycle_msgs/msg/state.hpp>
 
+#include <modulo_core/exceptions.hpp>
 #include <modulo_core/translators/message_readers.hpp>
+#include <stdexcept>
 
 template<class... Ts>
 struct overloaded : Ts... {
@@ -539,6 +543,58 @@ void BaseControllerInterface::add_service(
   }
 }
 
+void BaseControllerInterface::add_tf_listener() {
+  if (!is_node_initialized()) {
+    throw modulo_core::exceptions::CoreException("Failed to add TF buffer and listener: Node is not initialized yet.");
+  }
+  if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
+    RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF buffer and listener.");
+    console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE);
+    this->tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_node()->get_clock());
+    this->tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*this->tf_buffer_);
+  } else {
+    RCLCPP_DEBUG(this->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 = this->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 =
+      this->lookup_ros_transform(frame, reference_frame, tf2::TimePoint(std::chrono::microseconds(0)), duration);
+  if (validity_period > 0.0
+      && (this->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 (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) {
+    throw modulo_core::exceptions::LookupTransformException(
+        "Failed to lookup transform: No TF buffer / listener configured.");
+  }
+  try {
+    return this->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()));
+  }
+}
+
 rclcpp::QoS BaseControllerInterface::get_qos() const {
   return qos_;
 }
@@ -555,4 +611,13 @@ std::timed_mutex& BaseControllerInterface::get_command_mutex() {
   return command_mutex_;
 }
 
+bool BaseControllerInterface::is_node_initialized() const {
+  try {
+    get_node();
+    return true;
+  } catch (const std::runtime_error&) {
+    return false;
+  }
+}
+
 }// namespace modulo_controllers