From 55de935dd5ce3b7b8c76e5355c076fb3a91b183c Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Mon, 27 Jan 2025 08:56:05 +0100 Subject: [PATCH 1/4] Reapply "feat(controllers): add TF listener and lookup TF helpers in BaseControllerInterface (#169)" This reverts commit d8343e18dd205037c7644bb47335440fe3ac9799. --- CHANGELOG.md | 1 + .../src/ComponentInterface.cpp | 2 +- .../BaseControllerInterface.hpp | 58 +++++++++++++++++ .../src/BaseControllerInterface.cpp | 64 +++++++++++++++++++ 4 files changed, 124 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 60481229..886480c6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,7 @@ Release Versions: ## Upcoming changes - feat(controllers)!: remove robot description parameter (#186) +- feat(controllers)!: add TF listener interface in BaseControllerInterface (#169) ## 5.4.1 diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index cc0b7c15..59a46cd9 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/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); diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index 8aa8e033..e4447670 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include @@ -326,6 +328,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 @@ -350,6 +385,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 @@ -476,6 +517,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 @@ -511,6 +566,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 8ce1c8ff..3037e423 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -1,7 +1,10 @@ #include "modulo_controllers/BaseControllerInterface.hpp" +#include + #include +#include #include template @@ -515,6 +518,58 @@ void BaseControllerInterface::add_service_lockfree( this->create_service(service_name, callback); } +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_; } @@ -531,4 +586,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 From 8f0681c3b35abc97b4d4aa5fc212bd9275432b8b Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Mon, 27 Jan 2025 08:56:46 +0100 Subject: [PATCH 2/4] Reapply "feat(controllers): add TF broadcaster in BaseControllerInterface (#170)" This reverts commit dd0fc7ab842e5992162a05f6cbda9c0a5352afa7. --- CHANGELOG.md | 1 + .../BaseControllerInterface.hpp | 84 ++++++++++++++++++- .../src/BaseControllerInterface.cpp | 43 ++++++++++ 3 files changed, 126 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 886480c6..cac99baf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,6 +30,7 @@ Release Versions: - feat(controllers)!: remove robot description parameter (#186) - feat(controllers)!: add TF listener interface in BaseControllerInterface (#169) +- feat(controllers)!: add TF broadcaster in BaseControllerInterface (#170) ## 5.4.1 diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index e4447670..ae6ff415 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -9,6 +9,8 @@ #include #include #include +#include +#include #include #include @@ -361,6 +363,40 @@ class BaseControllerInterface : public controller_interface::ControllerInterface 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& 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& transforms); + /** * @brief Getter of the Quality of Service attribute. * @return The Quality of Service attribute @@ -531,6 +567,18 @@ class BaseControllerInterface : public controller_interface::ControllerInterface 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 + void publish_transforms( + const std::vector& transforms, const std::shared_ptr& tf_broadcaster, + bool is_static = false); + state_representation::ParameterMap parameter_map_;///< ParameterMap for handling parameters std::unordered_map read_only_parameters_; std::shared_ptr @@ -567,8 +615,10 @@ class BaseControllerInterface : public controller_interface::ControllerInterface std::map> custom_input_configuration_callables_; - std::shared_ptr tf_buffer_; ///< TF buffer - std::shared_ptr tf_listener_;///< TF listener + std::shared_ptr tf_buffer_; ///< TF buffer + std::shared_ptr tf_listener_; ///< TF listener + std::shared_ptr tf_broadcaster_; ///< TF broadcaster + std::shared_ptr static_tf_broadcaster_;///< TF static broadcaster }; template @@ -1015,4 +1065,34 @@ BaseControllerInterface::create_service(const std::string& service_name, const s } } +template +inline void BaseControllerInterface::publish_transforms( + const std::vector& transforms, const std::shared_ptr& tf_broadcaster, + bool is_static) { + if (!is_node_initialized()) { + throw modulo_core::exceptions::CoreException("Failed send transform(s): Node is not initialized yet."); + } + 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 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 diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 3037e423..72f8639e 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -570,6 +570,49 @@ geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transfo } } +void BaseControllerInterface::add_tf_broadcaster() { + if (!is_node_initialized()) { + throw modulo_core::exceptions::CoreException("Failed to add TF broadcaster: Node is not initialized yet."); + } + if (this->tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + this->tf_broadcaster_ = std::make_shared(get_node()); + } else { + RCLCPP_DEBUG(this->get_node()->get_logger(), "TF broadcaster already exists."); + } +} + +void BaseControllerInterface::add_static_tf_broadcaster() { + if (!is_node_initialized()) { + throw modulo_core::exceptions::CoreException("Failed to add static TF broadcaster: Node is not initialized yet."); + } + if (this->static_tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding static TF broadcaster."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + this->static_tf_broadcaster_ = std::make_shared(get_node()); + } else { + RCLCPP_DEBUG(this->get_node()->get_logger(), "Static TF broadcaster already exists."); + } +} + +void BaseControllerInterface::send_transform(const state_representation::CartesianPose& transform) { + this->send_transforms(std::vector{transform}); +} + +void BaseControllerInterface::send_transforms(const std::vector& transforms) { + this->publish_transforms(transforms, this->tf_broadcaster_); +} + +void BaseControllerInterface::send_static_transform(const state_representation::CartesianPose& transform) { + this->send_static_transforms(std::vector{transform}); +} + +void BaseControllerInterface::send_static_transforms( + const std::vector& transforms) { + this->publish_transforms(transforms, this->static_tf_broadcaster_, true); +} + rclcpp::QoS BaseControllerInterface::get_qos() const { return qos_; } From 5de69af888bcb18d1e13669c57b273acf6871e54 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Mon, 27 Jan 2025 08:57:15 +0100 Subject: [PATCH 3/4] Reapply "test(controllers): add TF listener and broadcaster tests (#172)" This reverts commit 25e34a61f950258249511698730bb1ffdcd48ae2. --- CHANGELOG.md | 1 + .../test/test_controller_interface.cpp | 74 ++++++++++++++++++- 2 files changed, 74 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index cac99baf..acd0adee 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -31,6 +31,7 @@ Release Versions: - 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 diff --git a/source/modulo_controllers/test/test_controller_interface.cpp b/source/modulo_controllers/test/test_controller_interface.cpp index 67cf4f05..adb4d1cb 100644 --- a/source/modulo_controllers/test/test_controller_interface.cpp +++ b/source/modulo_controllers/test/test_controller_interface.cpp @@ -18,7 +18,15 @@ class FriendControllerInterface : public ControllerInterface { public: using ControllerInterface::add_input; using ControllerInterface::add_output; + using ControllerInterface::add_static_tf_broadcaster; + using ControllerInterface::add_tf_broadcaster; + using ControllerInterface::add_tf_listener; + using ControllerInterface::lookup_transform; using ControllerInterface::read_input; + using ControllerInterface::send_static_transform; + using ControllerInterface::send_static_transforms; + using ControllerInterface::send_transform; + using ControllerInterface::send_transforms; using ControllerInterface::write_output; private: @@ -211,7 +219,71 @@ TYPED_TEST_P(ControllerInterfaceTest, OutputTest) { } } -REGISTER_TYPED_TEST_CASE_P(ControllerInterfaceTest, ConfigureErrorTest, InputTest, OutputTest); +TYPED_TEST_P(ControllerInterfaceTest, TF) { + EXPECT_THROW(this->interface_->add_tf_broadcaster(), modulo_core::exceptions::CoreException); + EXPECT_THROW(this->interface_->add_static_tf_broadcaster(), modulo_core::exceptions::CoreException); + EXPECT_THROW(this->interface_->add_tf_listener(), modulo_core::exceptions::CoreException); + auto send_early_tf = state_representation::CartesianPose::Random("controller_test", "world"); + EXPECT_THROW(this->interface_->send_transform(send_early_tf), modulo_core::exceptions::CoreException); + this->init(); + this->interface_->add_tf_broadcaster(); + this->interface_->add_static_tf_broadcaster(); + this->interface_->add_tf_listener(); + auto send_tf = state_representation::CartesianPose::Random("controller_test", "world"); + EXPECT_NO_THROW(this->interface_->send_transform(send_tf)); + sleep(1); + state_representation::CartesianPose lookup_tf; + EXPECT_NO_THROW(lookup_tf = this->interface_->lookup_transform("controller_test", "world")); + auto identity = send_tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + + sleep(1); + EXPECT_THROW( + lookup_tf = this->interface_->lookup_transform("controller_test", "world", 0.1), + modulo_core::exceptions::LookupTransformException); + + auto send_static_tf = state_representation::CartesianPose::Random("controller_static_test", "world"); + EXPECT_NO_THROW(this->interface_->send_static_transform(send_static_tf)); + EXPECT_THROW( + auto throw_tf = this->interface_->lookup_transform("dummy", "world"), + modulo_core::exceptions::LookupTransformException); + + EXPECT_NO_THROW(lookup_tf = this->interface_->lookup_transform("controller_static_test", "world")); + identity = send_static_tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + + std::vector send_tfs; + send_tfs.reserve(3); + for (std::size_t idx = 0; idx < 3; ++idx) { + send_tfs.emplace_back( + state_representation::CartesianPose::Random("controller_test_" + std::to_string(idx), "world")); + } + EXPECT_NO_THROW(this->interface_->send_transforms(send_tfs)); + for (const auto& tf : send_tfs) { + lookup_tf = this->interface_->lookup_transform(tf.get_name(), tf.get_reference_frame()); + identity = tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + } + + std::vector send_static_tfs; + send_static_tfs.reserve(3); + for (std::size_t idx = 0; idx < 3; ++idx) { + send_static_tfs.emplace_back( + state_representation::CartesianPose::Random("controller_test_static_" + std::to_string(idx), "world")); + } + EXPECT_NO_THROW(this->interface_->send_static_transforms(send_static_tfs)); + for (const auto& tf : send_static_tfs) { + lookup_tf = this->interface_->lookup_transform(tf.get_name(), tf.get_reference_frame()); + identity = tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); + } +} + +REGISTER_TYPED_TEST_CASE_P(ControllerInterfaceTest, ConfigureErrorTest, InputTest, OutputTest, TF); typedef ::testing::Types SignalTypes; INSTANTIATE_TYPED_TEST_CASE_P(TestPrefix, ControllerInterfaceTest, SignalTypes); From c824b3371837e8bcdbd1bfe1c0b5fcdda05cb540 Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Wed, 8 Apr 2026 12:30:44 +0200 Subject: [PATCH 4/4] chore: remove is node initialized check (#245) --- .../BaseControllerInterface.hpp | 9 --- .../src/BaseControllerInterface.cpp | 74 +++++++------------ .../test/test_controller_interface.cpp | 8 +- 3 files changed, 32 insertions(+), 59 deletions(-) diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index ae6ff415..a4581df7 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -421,12 +421,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 @@ -1069,9 +1063,6 @@ template inline void BaseControllerInterface::publish_transforms( const std::vector& transforms, const std::shared_ptr& tf_broadcaster, bool is_static) { - if (!is_node_initialized()) { - throw modulo_core::exceptions::CoreException("Failed send transform(s): Node is not initialized yet."); - } std::string modifier = is_static ? "static " : ""; if (tf_broadcaster == nullptr) { RCLCPP_ERROR_STREAM_THROTTLE( diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 72f8639e..830805f8 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -135,7 +135,7 @@ void BaseControllerInterface::pre_set_parameters_callback(std::vectorvalidate_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()) { @@ -194,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", @@ -498,44 +498,41 @@ BaseControllerInterface::validate_service_name(const std::string& service_name, void BaseControllerInterface::add_service( const std::string& service_name, const std::function& callback) { - this->create_service(service_name, callback); + create_service(service_name, callback); } void BaseControllerInterface::add_service( const std::string& service_name, const std::function& callback) { - this->create_service(service_name, callback); + create_service(service_name, callback); } void BaseControllerInterface::add_service_lockfree( const std::string& service_name, const std::function& callback) { - this->create_service(service_name, callback); + create_service(service_name, callback); } void BaseControllerInterface::add_service_lockfree( const std::string& service_name, const std::function& callback) { - this->create_service(service_name, callback); + create_service(service_name, callback); } 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."); + 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); - this->tf_buffer_ = std::make_shared(this->get_node()->get_clock()); - this->tf_listener_ = std::make_shared(*this->tf_buffer_); + tf_buffer_ = std::make_shared(get_node()->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); } else { - RCLCPP_DEBUG(this->get_node()->get_logger(), "TF buffer and listener already exist."); + 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 = this->lookup_ros_transform(frame, reference_frame, time_point, 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; @@ -545,9 +542,9 @@ 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); + 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) { + && (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); @@ -558,12 +555,12 @@ state_representation::CartesianPose BaseControllerInterface::lookup_transform( 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) { + if (tf_buffer_ == nullptr || 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); + 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())); @@ -571,46 +568,40 @@ geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transfo } void BaseControllerInterface::add_tf_broadcaster() { - if (!is_node_initialized()) { - throw modulo_core::exceptions::CoreException("Failed to add TF broadcaster: Node is not initialized yet."); - } - if (this->tf_broadcaster_ == nullptr) { - RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding TF broadcaster."); + if (tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(get_node()->get_logger(), "Adding TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - this->tf_broadcaster_ = std::make_shared(get_node()); + tf_broadcaster_ = std::make_shared(get_node()); } else { - RCLCPP_DEBUG(this->get_node()->get_logger(), "TF broadcaster already exists."); + RCLCPP_DEBUG(get_node()->get_logger(), "TF broadcaster already exists."); } } void BaseControllerInterface::add_static_tf_broadcaster() { - if (!is_node_initialized()) { - throw modulo_core::exceptions::CoreException("Failed to add static TF broadcaster: Node is not initialized yet."); - } - if (this->static_tf_broadcaster_ == nullptr) { - RCLCPP_DEBUG(this->get_node()->get_logger(), "Adding 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); - this->static_tf_broadcaster_ = std::make_shared(get_node()); + static_tf_broadcaster_ = std::make_shared(get_node()); } else { - RCLCPP_DEBUG(this->get_node()->get_logger(), "Static TF broadcaster already exists."); + RCLCPP_DEBUG(get_node()->get_logger(), "Static TF broadcaster already exists."); } } void BaseControllerInterface::send_transform(const state_representation::CartesianPose& transform) { - this->send_transforms(std::vector{transform}); + send_transforms(std::vector{transform}); } void BaseControllerInterface::send_transforms(const std::vector& transforms) { - this->publish_transforms(transforms, this->tf_broadcaster_); + publish_transforms(transforms, tf_broadcaster_); } void BaseControllerInterface::send_static_transform(const state_representation::CartesianPose& transform) { - this->send_static_transforms(std::vector{transform}); + send_static_transforms(std::vector{transform}); } void BaseControllerInterface::send_static_transforms( const std::vector& transforms) { - this->publish_transforms(transforms, this->static_tf_broadcaster_, true); + publish_transforms(transforms, static_tf_broadcaster_, true); } rclcpp::QoS BaseControllerInterface::get_qos() const { @@ -629,13 +620,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 diff --git a/source/modulo_controllers/test/test_controller_interface.cpp b/source/modulo_controllers/test/test_controller_interface.cpp index adb4d1cb..2cd2be78 100644 --- a/source/modulo_controllers/test/test_controller_interface.cpp +++ b/source/modulo_controllers/test/test_controller_interface.cpp @@ -220,11 +220,11 @@ TYPED_TEST_P(ControllerInterfaceTest, OutputTest) { } TYPED_TEST_P(ControllerInterfaceTest, TF) { - EXPECT_THROW(this->interface_->add_tf_broadcaster(), modulo_core::exceptions::CoreException); - EXPECT_THROW(this->interface_->add_static_tf_broadcaster(), modulo_core::exceptions::CoreException); - EXPECT_THROW(this->interface_->add_tf_listener(), modulo_core::exceptions::CoreException); + EXPECT_THROW(this->interface_->add_tf_broadcaster(), std::runtime_error); + EXPECT_THROW(this->interface_->add_static_tf_broadcaster(), std::runtime_error); + EXPECT_THROW(this->interface_->add_tf_listener(), std::runtime_error); auto send_early_tf = state_representation::CartesianPose::Random("controller_test", "world"); - EXPECT_THROW(this->interface_->send_transform(send_early_tf), modulo_core::exceptions::CoreException); + EXPECT_THROW(this->interface_->send_transform(send_early_tf), std::runtime_error); this->init(); this->interface_->add_tf_broadcaster(); this->interface_->add_static_tf_broadcaster();