refactor(components): improve component error handling

CHANGELOG.md

@@ -42,6 +42,7 @@ Release Versions:
 - refactor(controllers): split up base class (#135)
 - release: use updated base image (#139)
 - fix(controllers): remove duplicate function (#140)
+- refactor(components): improve component error handling (#138)
 
 ## 4.2.2
 

source/modulo_components/component_descriptions/modulo_component.json

@@ -1,5 +1,5 @@
 {
-  "$schema": "https://raw.githubusercontent.com/aica-technology/api/main/docs/static/schemas/1-1-1/component.schema.json",
+  "$schema": "https://docs.aica.tech/schemas/1-1-1/component.schema.json",
   "name": "Component",
   "description": {
     "brief": "A wrapper for rclcpp::Node to simplify application composition through unified component interfaces.",

source/modulo_components/component_descriptions/modulo_lifecycle_component.json

@@ -1,5 +1,5 @@
 {
-  "$schema": "https://raw.githubusercontent.com/aica-technology/api/main/docs/static/schemas/1-1-1/component.schema.json",
+  "$schema": "https://docs.aica.tech/schemas/1-1-1/component.schema.json",
   "name": "Lifecycle Component",
   "description": {
     "brief": "A wrapper for rclcpp_lifecycle::LifecycleNode to simplify application composition through unified component interfaces while supporting lifecycle states and transitions.",
@@ -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"
-    }
   ]
 }

source/modulo_components/include/modulo_components/Component.hpp

@@ -68,6 +68,11 @@ class Component : public rclcpp::Node, public ComponentInterface {
       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.
@@ -81,6 +86,7 @@ class Component : public rclcpp::Node, public ComponentInterface {
   void on_execute();
 
   // TODO hide ROS methods
+  using ComponentInterface::cancel_step;
   using ComponentInterface::get_parameter;
   using ComponentInterface::create_output;
   using ComponentInterface::inputs_;

source/modulo_components/include/modulo_components/ComponentInterface.hpp

@@ -93,6 +93,11 @@ class ComponentInterface {
    */
   virtual void on_step_callback();
 
+  /**
+   * @brief Cancel the step timer.
+   */
+  void cancel_step();
+
   /**
    * @brief Add a parameter.
    * @details This method stores a pointer reference to an existing Parameter object in the local parameter map and
@@ -425,7 +430,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();
 

source/modulo_components/include/modulo_components/LifecycleComponent.hpp

@@ -118,6 +118,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'.
@@ -265,7 +270,10 @@ class LifecycleComponent : public rclcpp_lifecycle::LifecycleNode, public Compon
    */
   bool clear_signals();
 
+  bool has_error_;
+
   // TODO hide ROS methods
+  using ComponentInterface::cancel_step;
   using ComponentInterface::get_parameter;
   using ComponentInterface::create_output;
   using ComponentInterface::inputs_;
@@ -296,7 +304,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

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._cancel_step()

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()
@@ -114,6 +113,13 @@ def on_step_callback(self):
         """
         pass
 
+    def _cancel_step(self):
+        """
+        Cancel the step timer.
+        """
+        if self.__step_timer:
+            self.__step_timer.cancel()
+
     def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, read_only=False) -> None:
         """
         Add a parameter. This method stores either the name of the attribute corresponding to the parameter object or
@@ -893,6 +899,6 @@ 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.")

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
+                # TODO reset and finalize all properties
+                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()
@@ -288,7 +290,6 @@ def on_error(self, previous_state: LifecycleState) -> TransitionCallbackReturn:
             self.get_logger().error("Error processing failed! Entering into the finalized state.")
             # TODO reset and finalize all needed properties
             return TransitionCallbackReturn.ERROR
-        self.set_predicate("in_error_state", False)
         return TransitionCallbackReturn.SUCCESS
 
     def __handle_error(self) -> bool:
@@ -306,7 +307,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 +321,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 +392,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()

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() {
@@ -61,4 +62,10 @@ std::shared_ptr<state_representation::ParameterInterface>
 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->cancel_step();
+}
 }// namespace modulo_components

source/modulo_components/src/ComponentInterface.cpp

@@ -34,8 +34,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<double>("rate");
   this->period_ = 1.0 / this->rate_;
   this->step_timer_ = rclcpp::create_wall_timer(
@@ -76,6 +74,12 @@ void ComponentInterface::step() {}
 
 void ComponentInterface::on_step_callback() {}
 
+void ComponentInterface::cancel_step() {
+  if (this->step_timer_ != nullptr) {
+    this->step_timer_->cancel();
+  }
+}
+
 void ComponentInterface::add_parameter(
     const std::shared_ptr<state_representation::ParameterInterface>& parameter, const std::string& description,
     bool read_only) {
@@ -561,8 +565,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 {