Revert "feat(controllers): add TF listener and lookup TF helpers in B…

…aseControllerInterface (#169)"

This reverts commit d106ba8.

CHANGELOG.md

@@ -22,7 +22,6 @@ 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(components): get clproto message type from attribute (#175)
  - fix(components): add missing test case (#181)
 

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: No TF buffer / listener configured.");
+    throw exceptions::LookupTransformException("Failed to lookup transform: To 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,8 +7,6 @@
 #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>
 
@@ -312,39 +310,6 @@ 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
@@ -369,12 +334,6 @@ 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
@@ -488,20 +447,6 @@ 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>
@@ -537,9 +482,6 @@ 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,13 +2,9 @@
 
 #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... {
@@ -543,58 +539,6 @@ 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_;
 }
@@ -611,13 +555,4 @@ 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