Support deferred hardware interface and controllers' activation

Signed-off-by: Michel Hidalgo <[email protected]>

spot_ros2_control/README.md

@@ -74,3 +74,4 @@ Add the launch argument `spot_name:=<namespace>` if the ros2 control stack was l
 * `controllers_config`: If this argument is unset, a general purpose controller configuration will be loaded containing a forward position controller and a joint state publisher, that is filled appropriately based on whether or not the robot used (mock or real) has an arm. The forward state controller is also specified here. If you wish to load different controllers, this can be set here.
 * `robot_controller`: This is the name of the robot controller that will be started when the launchfile is called. The default is the simple forward position controller. The name must match a controller in the `controllers_config` file.
 * `launch_rviz`: If you do not want rviz to be launched, add the argument `launch_rviz:=False`.
+* `auto_start`: If you do not want hardware interfaces and controllers to be activated on launch, add the argument `auto_start:=False`.

spot_ros2_control/config/spot_default_controllers_with_arm.yaml

@@ -14,6 +14,9 @@ controller_manager:
     forward_state_controller:
       type: spot_controllers/ForwardStateController
 
+    hardware_components_initial_state:
+      inactive:
+        - SpotSystem
 
 forward_position_controller:
   ros__parameters:

spot_ros2_control/config/spot_default_controllers_without_arm.yaml

@@ -14,6 +14,9 @@ controller_manager:
     forward_state_controller:
       type: spot_controllers/ForwardStateController
 
+    hardware_components_initial_state:
+      inactive:
+        - SpotSystem
 
 forward_position_controller:
   ros__parameters:

spot_ros2_control/launch/spot_ros2_control.launch.py

@@ -105,6 +105,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     mock_arm: bool = IfCondition(LaunchConfiguration("mock_arm")).evaluate(context)
     spot_name: str = LaunchConfiguration("spot_name").perform(context)
     config_file: str = LaunchConfiguration("config_file").perform(context)
+    auto_start: bool = LaunchConfiguration("auto_start").perform(context)
 
     # Default parameters used in the URDF if not connected to a robot
     arm = mock_arm
@@ -174,22 +175,29 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
             remappings=[(f"/{tf_prefix}joint_states", f"/{tf_prefix}low_level/joint_states")],
         )
     )
-    ld.add_action(
-        Node(
-            package="controller_manager",
-            executable="spawner",
-            arguments=["joint_state_broadcaster", "-c", "controller_manager"],
-            namespace=spot_name,
+    if auto_start:
+        ld.add_action(
+            Node(
+                package="controller_manager",
+                executable="hardware_spawner",
+                arguments=["-c", "controller_manager", "--activate", "SpotSystem"],
+                namespace=spot_name,
+            )
         )
-    )
-    ld.add_action(
-        Node(
-            package="controller_manager",
-            executable="spawner",
-            arguments=[LaunchConfiguration("robot_controller"), "-c", "controller_manager"],
-            namespace=spot_name,
+
+        ld.add_action(
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=[
+                    "-c",
+                    "controller_manager",
+                    "joint_state_broadcaster",
+                    LaunchConfiguration("robot_controller"),
+                ],
+                namespace=spot_name,
+            )
         )
-    )
     # Generate rviz configuration file based on the chosen namespace
     ld.add_action(
         Node(
@@ -297,6 +305,11 @@ def generate_launch_description():
                 default_value=True,
                 description="Choose whether to launch the image publishers.",
             ),
+            DeclareLaunchArgument(
+                "auto_start",
+                default_value=True,
+                description="Choose whether to start hardware interfaces and controllers immediately or not.",
+            ),
         ]
         + declare_image_publisher_args()
     )