Fix Hardware spawner and add tests for it (#1759)

* Tests for hardware spawner. * Fully support spawning a list of hardware components * Use python3 coverage instead of ros2 run in tests * Actually check component's state after changing it * Update hardware_spawner's user documentation --------- Co-authored-by: Dr. Denis Štogl <[email protected]> Co-authored-by: Bence Magyar <[email protected]> Co-authored-by: Christoph Fröhlich <[email protected]> (cherry picked from commit 5dbd6d5) # Conflicts: # controller_manager/CMakeLists.txt
ros-controls · Oct 31, 2024 · 3afba42 · 3afba42
1 parent 5c42ccb
commit 3afba42
Show file tree

Hide file tree

Showing 4 changed files with 339 additions and 38 deletions.
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
@@ -190,6 +190,21 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+<<<<<<< HEAD
+=======
+  ament_add_gmock(test_hardware_spawner
+    test/test_hardware_spawner
+    TIMEOUT 120
+  )
+  target_link_libraries(test_hardware_spawner
+    controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
+  install(FILES test/test_controller_spawner_with_fallback_controllers.yaml
+    DESTINATION test)
+
+>>>>>>> 5dbd6d5 (Fix Hardware spawner and add tests for it (#1759))
   install(FILES test/test_controller_spawner_with_type.yaml
   DESTINATION test)
 

diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py
@@ -57,7 +57,7 @@ def has_service_names(node, node_name, node_namespace, service_names):
 def is_hardware_component_loaded(
     node, controller_manager, hardware_component, service_timeout=0.0
 ):
-    components = list_hardware_components(node, hardware_component, service_timeout).component
+    components = list_hardware_components(node, controller_manager, service_timeout).component
     return any(c.name == hardware_component for c in components)
 
 
@@ -82,34 +82,33 @@ def handle_set_component_state_service_call(
         )
 
 
-def activate_components(node, controller_manager_name, components_to_activate):
+def activate_component(node, controller_manager_name, component_to_activate):
     active_state = State()
     active_state.id = State.PRIMARY_STATE_ACTIVE
     active_state.label = "active"
-    for component in components_to_activate:
-        handle_set_component_state_service_call(
-            node, controller_manager_name, component, active_state, "activated"
-        )
+    handle_set_component_state_service_call(
+        node, controller_manager_name, component_to_activate, active_state, "activated"
+    )
 
 
-def configure_components(node, controller_manager_name, components_to_configure):
+def configure_component(node, controller_manager_name, component_to_configure):
     inactive_state = State()
     inactive_state.id = State.PRIMARY_STATE_INACTIVE
     inactive_state.label = "inactive"
-    for component in components_to_configure:
-        handle_set_component_state_service_call(
-            node, controller_manager_name, component, inactive_state, "configured"
-        )
+    handle_set_component_state_service_call(
+        node, controller_manager_name, component_to_configure, inactive_state, "configured"
+    )
 
 
 def main(args=None):
     rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
     parser = argparse.ArgumentParser()
-    activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True)
+    activate_or_configure_grp = parser.add_mutually_exclusive_group(required=True)
 
     parser.add_argument(
-        "hardware_component_name",
-        help="The name of the hardware component which should be activated.",
+        "hardware_component_names",
+        help="The name of the hardware components which should be activated.",
+        nargs="+",
     )
     parser.add_argument(
         "-c",
@@ -126,13 +125,13 @@ def main(args=None):
         type=float,
     )
     # add arguments which are mutually exclusive
-    activate_or_confiigure_grp.add_argument(
+    activate_or_configure_grp.add_argument(
         "--activate",
         help="Activates the given components. Note: Components are by default configured before activated. ",
         action="store_true",
         required=False,
     )
-    activate_or_confiigure_grp.add_argument(
+    activate_or_configure_grp.add_argument(
         "--configure",
         help="Configures the given components.",
         action="store_true",
@@ -141,9 +140,9 @@ def main(args=None):
 
     command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
     args = parser.parse_args(command_line_args)
+    hardware_components = args.hardware_component_names
     controller_manager_name = args.controller_manager
     controller_manager_timeout = args.controller_manager_timeout
-    hardware_component = [args.hardware_component_name]
     activate = args.activate
     configure = args.configure
 
@@ -156,24 +155,25 @@ def main(args=None):
             controller_manager_name = f"/{controller_manager_name}"
 
     try:
-        if not is_hardware_component_loaded(
-            node, controller_manager_name, hardware_component, controller_manager_timeout
-        ):
-            node.get_logger().warn(
-                bcolors.WARNING
-                + "Hardware Component is not loaded - state can not be changed."
-                + bcolors.ENDC
-            )
-        elif activate:
-            activate_components(node, controller_manager_name, hardware_component)
-        elif configure:
-            configure_components(node, controller_manager_name, hardware_component)
-        else:
-            node.get_logger().error(
-                'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
-            )
-            parser.print_help()
-            return 0
+        for hardware_component in hardware_components:
+            if not is_hardware_component_loaded(
+                node, controller_manager_name, hardware_component, controller_manager_timeout
+            ):
+                node.get_logger().warn(
+                    bcolors.WARNING
+                    + "Hardware Component is not loaded - state can not be changed."
+                    + bcolors.ENDC
+                )
+            elif activate:
+                activate_component(node, controller_manager_name, hardware_component)
+            elif configure:
+                configure_component(node, controller_manager_name, hardware_component)
+            else:
+                node.get_logger().error(
+                    'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag'
+                )
+                parser.print_help()
+                return 0
     except KeyboardInterrupt:
         pass
     except ServiceNotFoundError as err:

diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst
@@ -136,16 +136,20 @@ There are two scripts to interact with controller manager from launch files:
 .. code-block:: console
 
     $ ros2 run controller_manager hardware_spawner -h
-    usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name
+    usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT]
+                            (--activate | --configure)
+                            hardware_component_names [hardware_component_names ...]
 
     positional arguments:
-      hardware_component_name
-                            The name of the hardware component which should be activated.
+      hardware_component_names
+                            The name of the hardware components which should be activated.
 
     options:
       -h, --help            show this help message and exit
       -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER
                             Name of the controller manager ROS node
+      --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
+                            Time to wait for the controller manager
       --activate            Activates the given components. Note: Components are by default configured before activated.
       --configure           Configures the given components.