diff --git a/CHANGELOG.md b/CHANGELOG.md index ce1705c0..2b1d8e25 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 30beb678..bbf2c811 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/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); diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index 7eae0168..bde57816 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -7,8 +7,6 @@ #include #include #include -#include -#include #include @@ -312,39 +310,6 @@ class BaseControllerInterface : public controller_interface::ControllerInterface const std::string& service_name, const std::function& 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 read_only_parameters_; std::shared_ptr @@ -537,9 +482,6 @@ class BaseControllerInterface : public controller_interface::ControllerInterface custom_output_configuration_callables_; std::map> custom_input_configuration_callables_; - - std::shared_ptr tf_buffer_; ///< TF buffer - std::shared_ptr tf_listener_;///< TF listener }; template diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 2233ecca..b3aaa754 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -2,13 +2,9 @@ #include -#include - #include -#include #include -#include template 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(this->get_node()->get_clock()); - this->tf_listener_ = std::make_shared(*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