diff --git a/CHANGELOG.md b/CHANGELOG.md index c14f4eec..366df2d3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,6 +47,7 @@ Release Versions: - feat(utils): add binary reader and recorder for encoded states (#152) - feat!: add support for custom inputs and outputs (#133) - release: version v5.0.0-rc0009 (#155) +- refactor(components): improve component error handling (#138) ## 4.2.2 diff --git a/aica-package.toml b/aica-package.toml index 0b76ef93..65dbae29 100644 --- a/aica-package.toml +++ b/aica-package.toml @@ -1,7 +1,7 @@ #syntax=ghcr.io/aica-technology/package-builder:v1.1.1 [metadata] -version = "5.0.0-rc0009" +version = "5.0.0-rc0010" description = "Modular ROS 2 extension library for dynamic composition of components and controllers with the AICA robotics framework" [metadata.collection] diff --git a/source/modulo_components/component_descriptions/modulo_lifecycle_component.json b/source/modulo_components/component_descriptions/modulo_lifecycle_component.json index d8795503..e8167dd9 100644 --- a/source/modulo_components/component_descriptions/modulo_lifecycle_component.json +++ b/source/modulo_components/component_descriptions/modulo_lifecycle_component.json @@ -17,12 +17,5 @@ "parameter_type": "double", "default_value": "10.0" } - ], - "predicates": [ - { - "display_name": "In error state", - "description": "True if the component is in error state", - "predicate_name": "in_error_state" - } ] } \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index eade5f7b..440b2341 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -67,6 +67,11 @@ class Component : public rclcpp::Node, public ComponentInterface { const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic = "", bool fixed_topic = false, bool publish_on_step = true); + /** + * Set the in_error_state predicate to true and cancel the step timer. + */ + void raise_error() override; + private: /** * @brief Step function that is called periodically and publishes predicates, outputs, and evaluates daemon callbacks. @@ -82,6 +87,7 @@ class Component : public rclcpp::Node, public ComponentInterface { // TODO hide ROS methods using ComponentInterface::create_output; using ComponentInterface::evaluate_periodic_callbacks; + using ComponentInterface::finalize_interfaces; using ComponentInterface::get_parameter; using ComponentInterface::inputs_; using ComponentInterface::outputs_; diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 59751fd6..29b197a4 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -418,7 +418,7 @@ class ComponentInterface { void set_qos(const rclcpp::QoS& qos); /** - * @brief Put the component in error state by setting the 'in_error_state' predicate to true. + * @brief Notify an error in the component. */ virtual void raise_error(); @@ -437,6 +437,11 @@ class ComponentInterface { */ void evaluate_periodic_callbacks(); + /** + * @brief Finalize all interfaces. + */ + void finalize_interfaces(); + std::map> inputs_;///< Map of inputs std::map> outputs_; ///< Map of outputs std::map periodic_outputs_;///< Map of outputs with periodic publishing flag @@ -539,9 +544,9 @@ class ComponentInterface { const std::string& frame, const std::string& reference_frame, const tf2::TimePoint& time_point, const tf2::Duration& duration); - double rate_; ///< The component rate in Hz - double period_; ///< The componet period in s - std::mutex step_mutex_;///< Mutex for step callback + double rate_; ///< The component rate in Hz + double period_; ///< The componet period in s + std::mutex step_mutex_; ///< Mutex for step callback std::map predicates_;///< Map of predicates std::shared_ptr> diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index a97a4fad..7234fed7 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -116,6 +116,11 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon */ rclcpp_lifecycle::State get_lifecycle_state() const; + /** + * @brief Trigger the shutdown and error transitions. + */ + void raise_error() override; + private: /** * @brief Transition callback for state 'Configuring'. @@ -258,14 +263,12 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon */ bool deactivate_outputs(); - /** - * @brief Cleanup all inputs and outputs. - */ - bool clear_signals(); + bool has_error_; // TODO hide ROS methods using ComponentInterface::create_output; using ComponentInterface::evaluate_periodic_callbacks; + using ComponentInterface::finalize_interfaces; using ComponentInterface::get_parameter; using ComponentInterface::inputs_; using ComponentInterface::outputs_; @@ -314,7 +317,4 @@ inline void LifecycleComponent::add_output( RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); } } - -// TODO, if we raise error we need to manually call the lifecycle change state callback, -// call callback function that this service calls }// namespace modulo_components diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index f178eb90..ec5fbbe2 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -23,6 +23,7 @@ def __init__(self, node_name: str, *args, **kwargs): self.__started = False self.__execute_thread = None self.add_predicate("is_finished", False) + self.add_predicate("in_error_state", False) def _step(self): """ @@ -35,6 +36,7 @@ def _step(self): self._publish_predicates() except Exception as e: self.get_logger().error(f"Failed to execute step function: {e}", throttle_duration_sec=1.0) + self.raise_error() def execute(self): """ @@ -94,3 +96,11 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT, self._outputs[parsed_signal_name]["publisher"] = publisher except Exception as e: self.get_logger().error(f"Failed to add output '{signal_name}': {e}") + + def raise_error(self): + """ + Set the in_error_state predicate to true and cancel the step timer. + """ + super().raise_error() + self.set_predicate("in_error_state", True) + self._finalize_interfaces() diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 52d2e115..79d9a29a 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -72,11 +72,10 @@ def __init__(self, node_name: str, *args, **kwargs): self.__predicate_message = PredicateCollection() self.__predicate_message.node = self.get_fully_qualified_name() self.__predicate_message.type = PredicateCollection.COMPONENT - self.add_predicate("in_error_state", False) self._rate = self.get_parameter_value("rate") self._period = 1.0 / self._rate - self.create_timer(self._period, self.__step_with_mutex) + self.__step_timer = self.create_timer(self._period, self.__step_with_mutex) def __del__(self): self.__step_lock.acquire() @@ -920,6 +919,22 @@ def __publish_transforms(self, transforms: Iterable[sr.CartesianPose], static=Fa def raise_error(self): """ - Put the component in error state by setting the 'in_error_state' predicate to true. + Notify an error in the component. """ - self.set_predicate("in_error_state", True) + self.get_logger().error("An error was raised in the component.") + + def _finalize_interfaces(self): + """ + Finalize all interfaces. + """ + self._inputs = {} + self._outputs = {} + self._services_dict = {} + self.__tf_buffer = None + self.__tf_listener = None + self.__tf_broadcaster = None + self.__static_tf_broadcaster = None + self._predicate_publisher = None + if self.__step_timer: + self.__step_timer.cancel() + self.__step_timer = None diff --git a/source/modulo_components/modulo_components/lifecycle_component.py b/source/modulo_components/modulo_components/lifecycle_component.py index 68b9e66e..0b9b55f5 100644 --- a/source/modulo_components/modulo_components/lifecycle_component.py +++ b/source/modulo_components/modulo_components/lifecycle_component.py @@ -26,6 +26,7 @@ def __init__(self, node_name: str, *args, **kwargs): lifecycle_node_kwargs = {key: value for key, value in kwargs.items() if key in LIFECYCLE_NODE_MIXIN_KWARGS} ComponentInterface.__init__(self, node_name, *args, **kwargs) LifecycleNodeMixin.__init__(self, *args, **lifecycle_node_kwargs) + self._has_error = False def get_lifecycle_state(self) -> LifecycleState: """ @@ -226,21 +227,23 @@ def on_shutdown(self, previous_state: LifecycleState) -> TransitionCallbackRetur TRANSITION_CALLBACK_FAILURE, TRANSITION_CALLBACK_ERROR or any uncaught exceptions to 'ErrorProcessing' """ self.get_logger().debug(f"on_shutdown called from previous state {previous_state.label}.") - if previous_state.state_id == State.PRIMARY_STATE_FINALIZED: - return TransitionCallbackReturn.SUCCESS - if previous_state.state_id == State.PRIMARY_STATE_ACTIVE: - if not self.__handle_deactivate(): - self.get_logger().debug("Shutdown failed during intermediate deactivation!") - if previous_state.state_id == State.PRIMARY_STATE_INACTIVE: - if not self.__handle_cleanup(): - self.get_logger().debug("Shutdown failed during intermediate cleanup!") - if previous_state.state_id == State.PRIMARY_STATE_UNCONFIGURED: - if not self.__handle_shutdown(): - self.get_logger().error("Entering into the error processing transition state.") - return TransitionCallbackReturn.ERROR - # TODO reset and finalize all properties - return TransitionCallbackReturn.SUCCESS - self.get_logger().warn(f"Invalid transition 'shutdown' from state {previous_state.label}.") + if not self._has_error: + if previous_state.state_id == State.PRIMARY_STATE_FINALIZED: + return TransitionCallbackReturn.SUCCESS + if previous_state.state_id == State.PRIMARY_STATE_ACTIVE: + if not self.__handle_deactivate(): + self.get_logger().debug("Shutdown failed during intermediate deactivation!") + if previous_state.state_id == State.PRIMARY_STATE_INACTIVE: + if not self.__handle_cleanup(): + self.get_logger().debug("Shutdown failed during intermediate cleanup!") + if previous_state.state_id == State.PRIMARY_STATE_UNCONFIGURED: + if not self.__handle_shutdown(): + self.get_logger().error("Entering into the error processing transition state.") + return TransitionCallbackReturn.ERROR + self._finalize_interfaces() + return TransitionCallbackReturn.SUCCESS + self.get_logger().warn(f"Invalid transition 'shutdown' from state {previous_state.label}.") + self.get_logger().error("Entering into the error processing transition state.") return TransitionCallbackReturn.ERROR def __handle_shutdown(self) -> bool: @@ -277,7 +280,6 @@ def on_error(self, previous_state: LifecycleState) -> TransitionCallbackReturn: TRANSITION_CALLBACK_ERROR should not be returned, and any exceptions should be caught and returned as a failure """ self.get_logger().debug(f"on_error called from previous state {previous_state.label}.") - self.set_predicate("in_error_state", True) error_handled = False try: error_handled = self.__handle_error() @@ -286,9 +288,9 @@ def on_error(self, previous_state: LifecycleState) -> TransitionCallbackReturn: error_handled = False if not error_handled: self.get_logger().error("Error processing failed! Entering into the finalized state.") - # TODO reset and finalize all needed properties + self._finalize_interfaces() return TransitionCallbackReturn.ERROR - self.set_predicate("in_error_state", False) + self._has_error = False return TransitionCallbackReturn.SUCCESS def __handle_error(self) -> bool: @@ -306,7 +308,7 @@ def on_error_callback(self) -> bool: :return: True if error handling is successful, false otherwise """ - return True + return False def _step(self): """ @@ -320,8 +322,8 @@ def _step(self): self._publish_outputs() self._publish_predicates() except Exception as e: - self.get_logger().error(f"Failed to execute step function: {e}", throttle_duration_sec=1.0) - # TODO handle error in lifecycle component + self.get_logger().error(f"Failed to execute step function: {e}") + self.raise_error() def __configure_outputs(self) -> bool: """ @@ -391,3 +393,11 @@ def __deactivate_outputs(self): self.get_logger().error(f"Failed to deactivate output '{signal_name}': {e}") self.get_logger().debug("All outputs deactivated.") return success + + def raise_error(self): + """ + Trigger the shutdown and error transitions. + """ + super().raise_error() + self._has_error = True + self.trigger_shutdown() diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 310d7f54..cb0ab8af 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -15,6 +15,7 @@ Component::Component(const NodeOptions& node_options, const std::string& fallbac Node::get_node_type_descriptions_interface(), Node::get_node_waitables_interface())), started_(false) { this->add_predicate("is_finished", false); + this->add_predicate("in_error_state", false); } void Component::step() { @@ -60,4 +61,10 @@ bool Component::on_execute_callback() { std::shared_ptr Component::get_parameter(const std::string& name) const { return ComponentInterface::get_parameter(name); } + +void Component::raise_error() { + ComponentInterface::raise_error(); + this->set_predicate("in_error_state", true); + this->finalize_interfaces(); +} }// namespace modulo_components diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 191fc528..bbf2c811 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -33,8 +33,6 @@ ComponentInterface::ComponentInterface( this->predicate_message_.node = this->node_base_->get_fully_qualified_name(); this->predicate_message_.type = modulo_interfaces::msg::PredicateCollection::COMPONENT; - this->add_predicate("in_error_state", false); - this->rate_ = this->get_parameter_value("rate"); this->period_ = 1.0 / this->rate_; this->step_timer_ = rclcpp::create_wall_timer( @@ -557,8 +555,7 @@ void ComponentInterface::set_qos(const rclcpp::QoS& qos) { } void ComponentInterface::raise_error() { - RCLCPP_DEBUG(this->node_logging_->get_logger(), "raise_error called: Setting predicate 'in_error_state' to true."); - this->set_predicate("in_error_state", true); + RCLCPP_ERROR(this->node_logging_->get_logger(), "An error was raised in the component."); } modulo_interfaces::msg::Predicate ComponentInterface::get_predicate_message(const std::string& name, bool value) const { @@ -611,4 +608,21 @@ void ComponentInterface::evaluate_periodic_callbacks() { } } } + +void ComponentInterface::finalize_interfaces() { + RCLCPP_DEBUG(this->node_logging_->get_logger(), "Finalizing all interfaces."); + this->inputs_.clear(); + this->outputs_.clear(); + this->predicate_publisher_.reset(); + this->empty_services_.clear(); + this->string_services_.clear(); + if (this->step_timer_ != nullptr) { + this->step_timer_->cancel(); + } + this->step_timer_.reset(); + this->tf_buffer_.reset(); + this->tf_listener_.reset(); + this->tf_broadcaster_.reset(); + this->static_tf_broadcaster_.reset(); +} }// namespace modulo_components diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 2b5c503a..00100444 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -13,7 +13,8 @@ LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options, LifecycleNode::get_node_parameters_interface(), LifecycleNode::get_node_services_interface(), LifecycleNode::get_node_time_source_interface(), LifecycleNode::get_node_timers_interface(), LifecycleNode::get_node_topics_interface(), LifecycleNode::get_node_type_descriptions_interface(), - LifecycleNode::get_node_waitables_interface())) {} + LifecycleNode::get_node_waitables_interface())), + has_error_(false) {} std::shared_ptr LifecycleComponent::get_parameter(const std::string& name) const { @@ -30,8 +31,7 @@ void LifecycleComponent::step() { this->publish_predicates(); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to execute step function: " << ex.what()); - // TODO handle error in lifecycle component - // this->raise_error(); + this->raise_error(); } } @@ -161,34 +161,32 @@ bool LifecycleComponent::on_deactivate_callback() { node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_shutdown(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_shutdown called from previous state %s", previous_state.label().c_str()); - switch (previous_state.id()) { - case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: - return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: - if (!this->handle_deactivate()) { - RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate deactivation!"); - break; - } - [[fallthrough]]; - case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: - if (!this->handle_cleanup()) { - RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate cleanup!"); - break; - } - [[fallthrough]]; - case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: - if (!this->handle_shutdown()) { + if (!this->has_error_) { + switch (previous_state.id()) { + case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: + if (!this->handle_deactivate()) { + RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate deactivation!"); + break; + } + [[fallthrough]]; + case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: + if (!this->handle_cleanup()) { + RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate cleanup!"); + break; + } + [[fallthrough]]; + case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: + if (!this->handle_shutdown()) { + break; + } + this->finalize_interfaces(); + return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + default: + RCLCPP_WARN(get_logger(), "Invalid transition 'shutdown' from state %s.", previous_state.label().c_str()); break; - } - // TODO: reset and finalize all needed properties - // this->handlers_.clear(); - // this->daemons_.clear(); - // this->parameters_.clear(); - // this->shutdown_ = true; - return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - default: - RCLCPP_WARN(get_logger(), "Invalid transition 'shutdown' from state %s.", previous_state.label().c_str()); - break; + } } RCLCPP_ERROR(get_logger(), "Entering into the error processing transition state."); return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; @@ -202,7 +200,7 @@ bool LifecycleComponent::handle_shutdown() { RCLCPP_ERROR_STREAM(get_logger(), ex.what()); return false; } - return result && this->clear_signals(); + return result; } bool LifecycleComponent::on_shutdown_callback() { @@ -211,7 +209,6 @@ bool LifecycleComponent::on_shutdown_callback() { node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_error(const State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_error called from previous state %s", previous_state.label().c_str()); - this->set_predicate("in_error_state", true); bool error_handled; try { error_handled = this->handle_error(); @@ -221,10 +218,10 @@ node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_e } if (!error_handled) { RCLCPP_ERROR(get_logger(), "Error processing failed! Entering into the finalized state."); - // TODO: reset and finalize all needed properties + this->finalize_interfaces(); return node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - this->set_predicate("in_error_state", false); + this->has_error_ = false; return node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -233,7 +230,7 @@ bool LifecycleComponent::handle_error() { } bool LifecycleComponent::on_error_callback() { - return true; + return false; } bool LifecycleComponent::configure_outputs() { @@ -305,13 +302,6 @@ bool LifecycleComponent::configure_outputs() { return success; } -bool LifecycleComponent::clear_signals() { - RCLCPP_DEBUG(this->get_logger(), "Clearing all inputs and outputs."); - this->inputs_.clear(); - this->outputs_.clear(); - return true; -} - bool LifecycleComponent::activate_outputs() { bool success = true; for (auto const& [name, interface] : this->outputs_) { @@ -343,4 +333,16 @@ bool LifecycleComponent::deactivate_outputs() { rclcpp_lifecycle::State LifecycleComponent::get_lifecycle_state() const { return this->get_current_state(); } + +void LifecycleComponent::raise_error() { + ComponentInterface::raise_error(); + this->has_error_ = true; + if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + this->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN); + } else if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + this->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_INACTIVE_SHUTDOWN); + } else if (get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + this->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVE_SHUTDOWN); + } +} }// namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 305fce69..7e57e050 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -307,12 +307,6 @@ TYPED_TEST(ComponentInterfaceTest, GetSetQoS) { EXPECT_EQ(qos, this->component_->get_qos()); } -TYPED_TEST(ComponentInterfaceTest, RaiseError) { - EXPECT_FALSE(this->component_->get_predicate("in_error_state")); - this->component_->raise_error(); - EXPECT_TRUE(this->component_->get_predicate("in_error_state")); -} - TYPED_TEST(ComponentInterfaceTest, AddTrigger) { EXPECT_NO_THROW(this->component_->add_trigger("trigger")); ASSERT_FALSE( diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 4c9ea809..99c02424 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -230,12 +230,6 @@ def test_get_set_qos(component_interface): assert qos == component_interface.get_qos() -def test_raise_error(component_interface): - assert not component_interface.get_predicate("in_error_state") - component_interface.raise_error() - assert component_interface.get_predicate("in_error_state") - - def test_add_trigger(component_interface): component_interface.add_trigger("trigger") assert "trigger" in component_interface._triggers diff --git a/source/modulo_controllers/src/BaseControllerInterface.cpp b/source/modulo_controllers/src/BaseControllerInterface.cpp index e07ef5e2..c10c2b89 100644 --- a/source/modulo_controllers/src/BaseControllerInterface.cpp +++ b/source/modulo_controllers/src/BaseControllerInterface.cpp @@ -41,7 +41,6 @@ BaseControllerInterface::on_configure(const rclcpp_lifecycle::State&) { add_outputs(); if (predicates_.size()) { - // TODO: topic predicate_publisher_ = get_node()->create_publisher("/predicates", qos_); predicate_message_.node = get_node()->get_fully_qualified_name();