feat(): revamped controller startup

.gitignore

@@ -1 +1,2 @@
 .ccache
+**/__pycache__/**

CMakeLists.txt

@@ -19,6 +19,9 @@ install(DIRECTORY
 install(
   PROGRAMS
     scripts/param_loader_node.py
+    scripts/hardware_activator_node.py
+    scripts/controller_activator_node.py
+    scripts/joint_state_merger_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

README.md

@@ -4,71 +4,145 @@
 [![Kilted Build Main](https://github.com/Duatic/duatic_control/actions/workflows/build-kilted.yml/badge.svg?branch=main)](https://github.com/Duatic/duatic_control/actions/workflows/build-kilted.yml)
 [![Rolling Build Main](https://github.com/Duatic/duatic_control/actions/workflows/build-rolling.yml/badge.svg?branch=main)](https://github.com/Duatic/duatic_control/actions/workflows/build-rolling.yml)
 
-Compact ros2_control integration for Duatic robots.
+Compact `ros2_control` integration for Duatic robots.
 
 ## Overview
-- The URDF plugin (urdf/plugins.urdf.xacro) expects a full path to a controllers YAML file.
-- That YAML must contain the controller_manager and controller node parameter mappings using the top-level wildcard `/**:` so the ros2_control plugin can load controllers on startup.
 
-## Quick usage
-- Ensure a config file exists and pass its absolute path to the launch/spawn process that loads the URDF plugin.
+This package provides:
+
+- `launch/control.launch.py`, a common launch sequence for controller manager setup and controller activation.
+- `scripts/param_loader_node.py`, which loads nested controller parameters into a running controller manager.
+- `scripts/hardware_activator_node.py`, which activates hardware components listed as initially inactive.
+- `scripts/controller_activator_node.py`, which activates all requested controllers in one strict `switch_controller` call.
+- `urdf/plugins.urdf.xacro`, a Gazebo Sim `gz_ros2_control` plugin macro.
+
+The launch sequence is intentionally staged:
+
+1. Start `ros2_control_node` unless `use_sim_time:=true`.
+2. Load controller manager and controller parameters from the YAML file.
+3. Spawn every configured controller inactive.
+4. Activate hardware components listed under `hardware_components_initial_state.inactive`.
+5. Activate controllers whose config metadata has `state: active`.
+
+This keeps hardware writes disabled until controllers have been loaded and configured, while still activating command-claiming controllers only after their hardware interfaces are available.
+
+## Quick Usage
+
+Pass the controller config with the `config_path` launch argument:
 
-Example:
 ```bash
 ros2 launch duatic_control control.launch.py \
-  namespace:="robot1" \
-  controllers_file:="/full/path/to/controllers.yaml"
+  namespace:=robot1 \
+  config_path:=/full/path/to/controllers.yaml
 ```
 
-## Config file format
-- File must be valid YAML.
-- Top-level must use `/**:` to scope parameters for controller_manager and named controllers.
-- Each controller definition can include:
-  - `type`: (required) The controller plugin type (e.g., `joint_state_broadcaster/JointStateBroadcaster`).
-  - `state`: (optional) Activation status on startup. Use `active` (default) or `inactive`.
-  - `remappings`: (optional) A dictionary of topic remappings in the format `topic_name: remapped_topic_name`.
+For Gazebo Sim, run the same launch file with `use_sim_time:=true`. In that mode `control.launch.py` does not start `ros2_control_node`; the Gazebo plugin owns the controller manager.
+
+```bash
+ros2 launch duatic_control control.launch.py \
+  namespace:=robot1 \
+  config_path:=/full/path/to/controllers_sim.yaml \
+  use_sim_time:=true
+```
+
+## Launch Arguments
+
+- `config_path`: Path to the controller config YAML file.
+- `namespace`: Optional namespace for controller manager, spawners, and activation nodes.
+- `use_sim_time`: When `false`, launch `controller_manager/ros2_control_node`. When `true`, skip it for Gazebo Sim.
+
+## Config File Format
+
+The config file must be valid YAML and must use `/**:` as the top-level scope:
 
-Example:
 ```yaml
 /**:
   controller_manager:
     ros__parameters:
-      update_rate: 100
+      update_rate: 1000
+
+      hardware_components_initial_state:
+        inactive:
+          - DuaTorsoSystem
+
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
         state: active
+
+      gravity_compensation_controller:
+        type: duatic_controllers/GravityCompensationController
+        state: active
+
+      joint_trajectory_controller_arm_left:
+        type: joint_trajectory_controller/JointTrajectoryController
+        state: inactive
+
+  joint_state_broadcaster:
+    ros__parameters:
+      use_urdf_to_filter: false
+      joints:
+        - arm_left/shoulder_rotation
+        - arm_left/shoulder_flexion
+
+  gravity_compensation_controller:
+    ros__parameters:
+      joints:
+        - arm_left/shoulder_rotation
+        - arm_left/shoulder_flexion
+```
+
+Controller entries under `controller_manager.ros__parameters` support:
+
+- `type`: Required controller plugin type.
+- `state`: Optional startup state metadata. Use `active` or `inactive`; default is `active`.
+- `remappings`: Optional dictionary of controller topic remappings.
+
+Example remapping:
+
+```yaml
+/**:
+  controller_manager:
+    ros__parameters:
       mecanum_drive_controller:
         type: mecanum_drive_controller/MecanumDriveController
         state: active
         remappings:
           mecanum_drive_controller/reference: cmd_vel
-      gravity_compensation_controller_arm_right:
-        type: dynaarm_controllers/GravityCompensationController
-        state: inactive
+```
 
-  mecanum_drive_controller:
+`state` and `remappings` are consumed by `control.launch.py` and are not uploaded as ROS parameters. All controllers are spawned with `--inactive`; controllers marked `active` are activated later by `controller_activator_node.py`.
+
+## Hardware Activation
+
+Hardware components can be listed as initially inactive:
+
+```yaml
+/**:
+  controller_manager:
     ros__parameters:
-      reference_timeout: 0.7
-      front_left_wheel_command_joint_name: "joint_wheel1"
-      front_right_wheel_command_joint_name: "joint_wheel2"
-      rear_right_wheel_command_joint_name: "joint_wheel3"
-      rear_left_wheel_command_joint_name: "joint_wheel4"
-      kinematics:
-        wheels_radius: 0.1015
-        sum_of_robot_center_projection_on_X_Y_axis: 0.595
-      enable_odom_tf: false
-      base_frame_id: "base_link"
+      hardware_components_initial_state:
+        inactive:
+          - DuaTorsoSystem
 ```
 
-## URDF plugin (for usage with Gazebo Sim)
-- Macro parameters: `namespace`, `controllers_file` (absolute path required).
-- The plugin reads the config file and injects it into the ros2_control controller_manager on spawn, the config file must be the same that's passed to the control.launch.py file.
+When this list is present, `hardware_activator_node.py` runs after all controller spawners finish and before controller activation. It calls the controller manager services to transition each listed component to `active`, configuring an `unconfigured` component first if needed.
+
+If no inactive hardware components are configured, hardware activation is skipped.
+
+## Gazebo Sim URDF Plugin
+
+Include the xacro macro in the robot description:
 
-Example:
 ```xml
 <xacro:include filename="$(find duatic_control)/urdf/plugins.urdf.xacro" />
-<xacro:control namespace="$(arg namespace)" controllers_file="$(find platform_bringup)/config/controllers.yaml"/>
+<xacro:control namespace="$(arg namespace)" />
 ```
 
+The macro adds the `gz_ros2_control::GazeboSimROS2ControlPlugin`, sets the namespace, and remaps common global topics into the namespace. The plugin loads the packaged `config/controller_manager.yaml`; controller-specific configuration is still provided through `control.launch.py config_path:=...`.
+
 ## Troubleshooting
-- If controllers don't load: validate YAML syntax, ensure `/**:` is present, and confirm the launch passes an absolute path to the controllers file.
+
+- If no controllers load, validate the YAML syntax and confirm `/**/controller_manager/ros__parameters` exists.
+- If the launch fails immediately, check that `config_path` points to an existing YAML file.
+- If a controller fails to activate, confirm its required hardware component was listed under `hardware_components_initial_state.inactive` when it should start inactive.
+- If Gazebo Sim already provides the controller manager, launch with `use_sim_time:=true` so `ros2_control_node` is not started twice.

launch/control.launch.py

@@ -24,6 +24,7 @@
 import yaml
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
+from launch.conditions import UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
@@ -44,6 +45,26 @@ def launch_setup(context, *args, **kwargs):
         print("Could not find '/**/controller_manager/ros__parameters' in the YAML file.")
         return []
 
+    # Extract controller manager parameters (update_rate, hardware_components_initial_state)
+    cm_parameters = []
+    update_rate = cm_params.pop("update_rate", 1000)
+    if update_rate is not None:
+        cm_parameters.append({"update_rate": update_rate})
+    hardware_components_initial_state = cm_params.pop("hardware_components_initial_state", {})
+    if hardware_components_initial_state:
+        cm_parameters.append(
+            {"hardware_components_initial_state": hardware_components_initial_state}
+        )
+
+    # Controller Manager Node
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=cm_parameters,
+        output={"stdout": "screen", "stderr": "screen"},
+        condition=UnlessCondition(LaunchConfiguration("use_sim_time")),
+    )
+
     # Identify controllers, states, and remappings
     controllers = []
 
@@ -59,8 +80,8 @@ def launch_setup(context, *args, **kwargs):
 
             controllers.append((controller_name, ctrl_state, ctrl_remappings))
 
-    # Spawn controllers
-    nodes = []
+    # Configure controllers
+    spawner_nodes = []
     for controller_name, state, remappings in controllers:
         args = [
             controller_name,
@@ -70,10 +91,8 @@ def launch_setup(context, *args, **kwargs):
             "30.0",
             "--param-file",
             config_path,
+            "--inactive",
         ]
-        # Add --inactive if controller should start inactive
-        if state == "inactive":
-            args.append("--inactive")
 
         # Handle Remappings
         if remappings:
@@ -88,7 +107,7 @@ def launch_setup(context, *args, **kwargs):
             arguments=args,
             output="screen",
         )
-        nodes.append(node)
+        spawner_nodes.append(node)
 
     # Parameter loader node
     target_node = "/controller_manager"
@@ -109,13 +128,55 @@ def launch_setup(context, *args, **kwargs):
         ],
     )
 
-    spawn_controller_after_loading_params = RegisterEventHandler(
-        OnProcessExit(
-            target_action=load_params, on_exit=nodes  # Spawn controllers after params loaded
+    # Chain: param_loader -> spawner[0] -> spawner[1] -> … -> hardware activator -> controller activator
+    prev = load_params
+    event_handlers = []
+    for spawner_node in spawner_nodes:
+        event_handlers.append(
+            RegisterEventHandler(OnProcessExit(target_action=prev, on_exit=[spawner_node]))
+        )
+        prev = spawner_node
+
+    active_names = [name for name, state, _ in controllers if state == "active"]
+    inactive_components = hardware_components_initial_state.get("inactive", [])
+
+    # Hardware must be active before its command interfaces can be claimed.
+    if inactive_components:
+        hardware_activator = Node(
+            package="duatic_control",
+            executable="hardware_activator_node.py",
+            name="hardware_activator",
+            output="screen",
+            parameters=[
+                {
+                    "controller_manager": target_node,
+                    "hardware_components": inactive_components,
+                }
+            ],
+        )
+        event_handlers.append(
+            RegisterEventHandler(OnProcessExit(target_action=prev, on_exit=[hardware_activator]))
+        )
+        prev = hardware_activator
+
+    if active_names:
+        controller_activator = Node(
+            package="duatic_control",
+            executable="controller_activator_node.py",
+            name="controller_activator",
+            output="screen",
+            parameters=[
+                {
+                    "controllers": active_names,
+                    "controller_manager": target_node,
+                }
+            ],
+        )
+        event_handlers.append(
+            RegisterEventHandler(OnProcessExit(target_action=prev, on_exit=[controller_activator]))
         )
-    )
 
-    return [load_params, spawn_controller_after_loading_params]
+    return [controller_manager, load_params] + event_handlers
 
 
 def generate_launch_description():
@@ -124,6 +185,7 @@ def generate_launch_description():
             "config_path", default_value="", description="Path to the controller config YAML file."
         ),
         DeclareLaunchArgument("namespace", default_value=""),
+        DeclareLaunchArgument("use_sim_time", default_value="false"),
     ]
 
     # Add nodes to LaunchDescription

package.xml

@@ -8,6 +8,7 @@
   <license>Duatic</license>
 
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>controller_manager_msgs</exec_depend>
   <exec_depend>gz_ros2_control</exec_depend>
   <exec_depend>xacro</exec_depend>