Merge branch 'master' into feat/jerk-sat-limit-refactor

Author: sachinkum0009

controller_interface/include/controller_interface/chainable_controller_interface.hpp

@@ -43,7 +43,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase
 
   /**
    * @brief Control step. Updates command interfaces from reference inputs and current states.
-   * **The method called in the (real-time) control loop.**
+   * @note This method needs to be real-time safe and thread-safe to be called in the control loop.
    *
    * @param[in] time The time at the start of this control loop iteration
    * @param[in] period The measured time taken by the last control loop iteration
@@ -133,6 +133,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase
 
   /**
    * @brief Update reference from input topics when not in chained mode.
+   * @note This method needs to be real-time safe and thread-safe to be called in the control loop.
    *
    * Each chainable controller implements this method to update reference from subscribers when not
    * in chained mode.
@@ -144,6 +145,7 @@ class ChainableControllerInterface : public ControllerInterfaceBase
 
   /**
    * @brief Execute calculations of the controller and update command interfaces.
+   * @note This method needs to be real-time safe and thread-safe to be called in the control loop.
    *
    * Update method for chainable controllers.
    * In this method is valid to assume that \reference_interfaces_ hold the values for calculation

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -193,7 +193,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   /**
    * @brief Control step update. Command interfaces are updated based on on reference inputs and
    * current states.
-   * **The method called in the (real-time) control loop.**
+   * @note This method needs to be real-time safe and thread-safe to be called in the control loop.
    *
    * @param[in] time The time at the start of this control loop iteration
    * @param[in] period The measured time since the last control loop iteration
@@ -205,7 +205,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * @brief Trigger update method. This method is used by the controller_manager to trigger the
    * update method of the controller. The method is used to trigger the update method of the
    * controller synchronously or asynchronously, based on the controller configuration.
-   * **The method called in the (real-time) control loop.**
+   * @note This method needs to be real-time safe and thread-safe to be called in the control loop.
    *
    * @param[in] time The time at the start of this control loop iteration
    * @param[in] period The measured time taken by the last control loop iteration
@@ -222,15 +222,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * @brief Get the current lifecycle state of the controller node.
    * @note Accessing members of the returned rclcpp_lifecycle::State is not real-time safe and
    * should not be called in the control loop.
-   * @note This method is thread safe.
+   * @note This method needs to be real-time safe and thread-safe to be called in the control loop.
+   *
    * @returns lifecycle state of the controller node.
    */
   const rclcpp_lifecycle::State & get_lifecycle_state() const;
 
   /**
    * @brief Get the lifecycle id of the controller node that is cached internally
    * to avoid calls to get_lifecycle_state() in the real-time control loop.
-   * @note This method is real-time safe and thread safe and can be called in the control loop.
+   * @note This method needs to be real-time safe and thread-safe to be called in the control loop.
+   *
    * @returns lifecycle id of the controller node.
    */
   uint8_t get_lifecycle_id() const;

rqt_controller_manager/rqt_controller_manager/controller_manager.py

@@ -147,6 +147,7 @@ def __init__(self, context):
     def shutdown_plugin(self):
         self._update_cm_list_timer.stop()
         self._update_ctrl_list_timer.stop()
+        self._update_hw_components_list_timer.stop()
         self._popup_widget.hide()
 
     def save_settings(self, plugin_settings, instance_settings):
@@ -165,7 +166,13 @@ def restore_settings(self, plugin_settings, instance_settings):
             pass
 
     def _update_cm_list(self):
-        update_combo(self._widget.cm_combo, _list_controller_managers(self._node))
+        try:
+            update_combo(self._widget.cm_combo, _list_controller_managers(self._node))
+        except Exception as e:
+            if "destruction was requested" in str(e) or "context is not valid" in str(e):
+                self._update_cm_list_timer.stop()
+                return
+            raise
 
     def _on_cm_change(self, cm_name):
         self._cm_name = cm_name
@@ -179,7 +186,13 @@ def _update_controllers(self):
             return
 
         # Find controllers associated to the selected controller manager
-        controllers = self._list_controllers()
+        try:
+            controllers = self._list_controllers()
+        except Exception as e:
+            if "destruction was requested" in str(e) or "context is not valid" in str(e):
+                self._update_ctrl_list_timer.stop()
+                return
+            raise
 
         # Update controller display, if necessary
         if self._controllers != controllers:
@@ -322,7 +335,13 @@ def _update_hw_components(self):
             return
 
         # Find hw_components associated to the selected controller manager
-        hw_components = self._list_hw_components()
+        try:
+            hw_components = self._list_hw_components()
+        except Exception as e:
+            if "destruction was requested" in str(e) or "context is not valid" in str(e):
+                self._update_hw_components_list_timer.stop()
+                return
+            raise
 
         # Update controller display, if necessary
         if self._hw_components != hw_components:
@@ -644,11 +663,16 @@ def _list_controller_managers(node):
     @return List of controller manager node names
     @rtype list of str
     """
-    return [
-        name.rstrip("list_controllers").rstrip("/")
-        for name, _ in get_service_names_and_types(node=node)
-        if name.endswith("list_controllers")
-    ]
+    try:
+        return [
+            name.rstrip("list_controllers").rstrip("/")
+            for name, _ in get_service_names_and_types(node=node)
+            if name.endswith("list_controllers")
+        ]
+    except Exception as e:
+        if "destruction was requested" in str(e) or "context is not valid" in str(e):
+            return []
+        raise
 
 
 def _get_parameter_controller_names(node, node_name):

rqt_controller_manager/test/test_run_launch.py

@@ -35,6 +35,7 @@ def generate_test_description():
                 executable="rqt_controller_manager",
                 package="rqt_controller_manager",
                 name=node_name,
+                arguments=["--force-discover"],
                 additional_env={"QT_QPA_PLATFORM": "offscreen"},
             ),
             launch_testing.actions.ReadyToTest(),