From af7a49b08324591d45665b4cefe4c0b104eb0302 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 15:07:27 +0100 Subject: [PATCH 1/2] fix: add helper function to check if node is initialized --- .../modulo_controllers/BaseControllerInterface.hpp | 6 ++++++ .../src/BaseControllerInterface.cpp | 13 +++++++++++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index c2e17ec4..0b50651c 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -405,6 +405,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 diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index 3fc9637f..f7bb4e31 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -8,6 +8,7 @@ #include #include +#include template struct overloaded : Ts... { @@ -543,10 +544,9 @@ void BaseControllerInterface::add_service( } void BaseControllerInterface::add_tf_listener() { - if (this->get_node() == nullptr) { + 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); @@ -654,4 +654,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 b2261eb5cb1b839e7d33ab962f75db65b5eed2f6 Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Thu, 5 Dec 2024 15:17:30 +0100 Subject: [PATCH 2/2] fix: for broadcasters too --- .../include/modulo_controllers/BaseControllerInterface.hpp | 2 +- source/modulo_controllers/src/BaseControllerInterface.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index 0b50651c..b26c19aa 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -978,7 +978,7 @@ template inline void BaseControllerInterface::publish_transforms( const std::vector& transforms, const std::shared_ptr& tf_broadcaster, bool is_static) { - if (this->get_node() == nullptr) { + if (!is_node_initialized()) { throw modulo_core::exceptions::CoreException("Failed send transform(s): Node is not initialized yet."); } std::string modifier = is_static ? "static " : ""; diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index f7bb4e31..562d3cfc 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -596,7 +596,7 @@ geometry_msgs::msg::TransformStamped BaseControllerInterface::lookup_ros_transfo } void BaseControllerInterface::add_tf_broadcaster() { - if (this->get_node() == nullptr) { + if (!is_node_initialized()) { throw modulo_core::exceptions::CoreException("Failed to add TF broadcaster: Node is not initialized yet."); } if (this->tf_broadcaster_ == nullptr) { @@ -609,7 +609,7 @@ void BaseControllerInterface::add_tf_broadcaster() { } void BaseControllerInterface::add_static_tf_broadcaster() { - if (this->get_node() == nullptr) { + 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) {