From b1e5c0f2541169284d5c8e4697b4c71ff0c70a8e Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 09:16:16 +0100 Subject: [PATCH] feat: add TF listener and lookup TF helpers in BaseControllerInterface --- CHANGELOG.md | 1 + .../BaseControllerInterface.hpp | 52 +++++++++++++++++++ .../src/BaseControllerInterface.cpp | 51 ++++++++++++++++++ 3 files changed, 104 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7945b27d..8d0290ac 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index bde57816..e442c99f 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -7,6 +7,8 @@ #include #include #include +#include +#include #include @@ -310,6 +312,39 @@ 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 @@ -447,6 +482,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 read_only_parameters_; std::shared_ptr @@ -482,6 +531,9 @@ 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 b3aaa754..795e5466 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -2,6 +2,8 @@ #include +#include + #include #include @@ -539,6 +541,55 @@ void BaseControllerInterface::add_service( } } +void BaseControllerInterface::add_tf_listener() { + 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: To 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_; }