diff --git a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp index 28c059f7..e60c5aa2 100644 --- a/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp +++ b/source/modulo_controllers/include/modulo_controllers/BaseControllerInterface.hpp @@ -450,12 +450,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 @@ -1182,9 +1176,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 46a188e3..8dcd460d 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -138,7 +138,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()) { @@ -197,7 +197,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", @@ -501,44 +501,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; @@ -548,9 +545,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); @@ -561,12 +558,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())); @@ -574,46 +571,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 { @@ -632,13 +623,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();