diff --git a/.gitignore b/.gitignore
index 2357054..8bc7c56 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,2 @@
.ccache
+**/__pycache__/**
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b5c5a48..d716ee4 100644
--- a/CMakeLists.txt
+++ b/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}
)
diff --git a/README.md b/README.md
index 0c78ba8..77824b9 100644
--- a/README.md
+++ b/README.md
@@ -4,71 +4,145 @@
[](https://github.com/Duatic/duatic_control/actions/workflows/build-kilted.yml)
[](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
-
+
```
+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.
diff --git a/launch/control.launch.py b/launch/control.launch.py
index 6370ece..a9041cd 100644
--- a/launch/control.launch.py
+++ b/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
diff --git a/package.xml b/package.xml
index f11fca6..0d00af9 100644
--- a/package.xml
+++ b/package.xml
@@ -8,6 +8,7 @@
Duatic
rclpy
+ controller_manager_msgs
gz_ros2_control
xacro
diff --git a/scripts/controller_activator_node.py b/scripts/controller_activator_node.py
new file mode 100644
index 0000000..69c385e
--- /dev/null
+++ b/scripts/controller_activator_node.py
@@ -0,0 +1,141 @@
+#!/usr/bin/env python3
+
+# Copyright 2026 Duatic AG
+#
+# Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+# the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this list of conditions, and
+# the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions, and
+# the following disclaimer in the documentation and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or
+# promote products derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
+# WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+# TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""Activate ROS 2 controllers through the controller manager."""
+
+import rclpy
+from rclpy.node import Node
+from controller_manager_msgs.srv import SwitchController
+
+
+class ControllerActivatorNode(Node):
+ """Activate a configured set of controllers.
+
+ The node reads controller names from the ``controllers`` parameter and sends
+ them in one strict ``switch_controller`` request to the configured controller
+ manager.
+
+ Attributes:
+ controllers: Names of controllers to activate.
+ cm_name: Controller manager node name or namespace.
+ """
+
+ def __init__(self):
+ """Initialize the node and read controller activation parameters."""
+ super().__init__("controller_activator")
+
+ self.declare_parameter("controllers", [""])
+ self.declare_parameter("controller_manager", "/controller_manager")
+
+ self.controllers = [
+ c
+ for c in self.get_parameter("controllers").get_parameter_value().string_array_value
+ if c
+ ]
+ self.cm_name = self.get_parameter("controller_manager").get_parameter_value().string_value
+
+ def _wait_for_service(self, client, name):
+ """Wait for a ROS service to become available.
+
+ Args:
+ client: ROS service client to wait on.
+ name: Human-readable service name used in log messages.
+
+ Returns:
+ True if the service becomes available before timeout, otherwise False.
+ """
+ attempts = 0
+ while not client.wait_for_service(timeout_sec=1.0):
+ attempts += 1
+ if attempts > 20:
+ self.get_logger().error(f"Timeout waiting for {name} service.")
+ return False
+ self.get_logger().warn(f"Waiting for {name} service... (attempt {attempts})")
+ return True
+
+ def _call(self, client, req):
+ """Call a ROS service synchronously using a spin loop.
+
+ Args:
+ client: ROS service client used to send the request.
+ req: Service request message.
+
+ Returns:
+ The service response, or None if the call fails.
+ """
+ future = client.call_async(req)
+ rclpy.spin_until_future_complete(self, future)
+ return future.result()
+
+ def activate_controllers(self):
+ """Activate all configured controllers.
+
+ Returns:
+ True if there are no controllers to activate or activation succeeds,
+ otherwise False.
+ """
+ if not self.controllers:
+ self.get_logger().info("No controllers to activate.")
+ return True
+
+ client = self.create_client(SwitchController, f"{self.cm_name}/switch_controller")
+ if not self._wait_for_service(client, "switch_controller"):
+ return False
+
+ self.get_logger().info(f"Activating controllers: {self.controllers}")
+ req = SwitchController.Request()
+ req.activate_controllers = self.controllers
+ req.strictness = SwitchController.Request.STRICT
+ req.activate_asap = True
+
+ result = self._call(client, req)
+ if result is None or not result.ok:
+ self.get_logger().error(f"Failed to activate controllers: {self.controllers}")
+ return False
+ self.get_logger().info(f"Successfully activated controllers: {self.controllers}")
+ return True
+
+ def run(self):
+ """Run the one-shot controller activation workflow."""
+ self.activate_controllers()
+
+
+def main(args=None):
+ """Run the controller activator node.
+
+ Args:
+ args: Optional ROS command-line arguments.
+ """
+ rclpy.init(args=args)
+ node = ControllerActivatorNode()
+ try:
+ node.run()
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/scripts/hardware_activator_node.py b/scripts/hardware_activator_node.py
new file mode 100644
index 0000000..541bf65
--- /dev/null
+++ b/scripts/hardware_activator_node.py
@@ -0,0 +1,198 @@
+#!/usr/bin/env python3
+
+# Copyright 2026 Duatic AG
+#
+# Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+# the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this list of conditions, and
+# the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions, and
+# the following disclaimer in the documentation and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or
+# promote products derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
+# WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+# TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""Activate ROS 2 control hardware components through the controller manager."""
+
+import rclpy
+from rclpy.node import Node
+from lifecycle_msgs.msg import State
+from controller_manager_msgs.srv import (
+ SetHardwareComponentState,
+ ListHardwareComponents,
+)
+
+
+class HardwareActivatorNode(Node):
+ """Bring requested hardware components to the active lifecycle state.
+
+ The node reads hardware component names from the ``hardware_components``
+ parameter and activates each component through the configured controller
+ manager. Components that are still unconfigured are first transitioned to
+ inactive, then active.
+
+ Attributes:
+ cm_name: Controller manager node name or namespace.
+ hardware_components: Names of hardware components to activate.
+ """
+
+ def __init__(self):
+ """Initialize the node and read hardware activation parameters."""
+ super().__init__("hardware_activator")
+
+ self.declare_parameter("controller_manager", "/controller_manager")
+ self.declare_parameter("hardware_components", [""])
+
+ self.cm_name = self.get_parameter("controller_manager").get_parameter_value().string_value
+ self.hardware_components = [
+ h
+ for h in self.get_parameter("hardware_components")
+ .get_parameter_value()
+ .string_array_value
+ if h
+ ]
+
+ def _wait_for_service(self, client, name):
+ """Wait for a ROS service to become available.
+
+ Args:
+ client: ROS service client to wait on.
+ name: Human-readable service name used in log messages.
+
+ Returns:
+ True if the service becomes available before timeout, otherwise False.
+ """
+ attempts = 0
+ while not client.wait_for_service(timeout_sec=1.0):
+ attempts += 1
+ if attempts > 20:
+ self.get_logger().error(f"Timeout waiting for {name} service.")
+ return False
+ self.get_logger().warn(f"Waiting for {name} service... (attempt {attempts})")
+ return True
+
+ def _call(self, client, req):
+ """Call a ROS service synchronously using a spin loop.
+
+ Args:
+ client: ROS service client used to send the request.
+ req: Service request message.
+
+ Returns:
+ The service response, or None if the call fails.
+ """
+ future = client.call_async(req)
+ rclpy.spin_until_future_complete(self, future)
+ return future.result()
+
+ def activate_hardware(self):
+ """Transition the requested hardware components to the active state.
+
+ Returns:
+ True if all configured components are active or activation succeeds,
+ otherwise False.
+ """
+ if not self.hardware_components:
+ self.get_logger().info(
+ "No hardware components configured for activation; skipping hardware activation."
+ )
+ return True
+
+ list_client = self.create_client(
+ ListHardwareComponents, f"{self.cm_name}/list_hardware_components"
+ )
+ set_client = self.create_client(
+ SetHardwareComponentState, f"{self.cm_name}/set_hardware_component_state"
+ )
+ if not self._wait_for_service(
+ list_client, "list_hardware_components"
+ ) or not self._wait_for_service(set_client, "set_hardware_component_state"):
+ return False
+
+ components = self._call(list_client, ListHardwareComponents.Request())
+ if components is None:
+ self.get_logger().error("Failed to list hardware components.")
+ return False
+
+ states = {c.name: c.state.id for c in components.component}
+
+ ok = True
+ for name in self.hardware_components:
+ current = states.get(name)
+ if current is None:
+ self.get_logger().error(f"Hardware component '{name}' not found.")
+ ok = False
+ continue
+ if current == State.PRIMARY_STATE_ACTIVE:
+ self.get_logger().info(f"Hardware component '{name}' already active.")
+ continue
+
+ # If the component is still unconfigured, configure it first, then activate.
+ if current == State.PRIMARY_STATE_UNCONFIGURED:
+ if not self._set_hardware_state(
+ set_client, name, State.PRIMARY_STATE_INACTIVE, "inactive"
+ ):
+ ok = False
+ continue
+
+ if not self._set_hardware_state(set_client, name, State.PRIMARY_STATE_ACTIVE, "active"):
+ ok = False
+
+ return ok
+
+ def _set_hardware_state(self, client, name, state_id, label):
+ """Set one hardware component to a requested lifecycle state.
+
+ Args:
+ client: Client for ``set_hardware_component_state``.
+ name: Hardware component name.
+ state_id: Lifecycle state ID from ``lifecycle_msgs.msg.State``.
+ label: Lifecycle state label used in the request and log messages.
+
+ Returns:
+ True if the controller manager reports success, otherwise False.
+ """
+ req = SetHardwareComponentState.Request()
+ req.name = name
+ req.target_state = State(id=state_id, label=label)
+ result = self._call(client, req)
+ if result is None or not result.ok:
+ self.get_logger().error(f"Failed to set '{name}' to '{label}'.")
+ return False
+ self.get_logger().info(f"Hardware component '{name}' -> '{label}'.")
+ return True
+
+ def run(self):
+ """Run the one-shot hardware activation workflow."""
+ if not self.activate_hardware():
+ self.get_logger().error("Aborting: hardware activation failed.")
+
+
+def main(args=None):
+ """Run the hardware activator node.
+
+ Args:
+ args: Optional ROS command-line arguments.
+ """
+ rclpy.init(args=args)
+ node = HardwareActivatorNode()
+ try:
+ node.run()
+ finally:
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/scripts/joint_state_merger_node.py b/scripts/joint_state_merger_node.py
new file mode 100644
index 0000000..0d78c16
--- /dev/null
+++ b/scripts/joint_state_merger_node.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+
+# Copyright 2026 Duatic AG
+#
+# Redistribution and use in source and binary forms, with or without modification, are permitted provided that
+# the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this list of conditions, and
+# the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions, and
+# the following disclaimer in the documentation and/or other materials provided with the distribution.
+#
+# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or
+# promote products derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
+# WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+# TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""Merge /joint_states_body and /joint_states_head into a single /joint_states topic."""
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+
+class JointStateMergerNode(Node):
+ def __init__(self):
+ super().__init__("joint_state_merger")
+
+ self._body_cache: dict[str, tuple[float, float, float]] = {}
+ self._head_cache: dict[str, tuple[float, float, float]] = {}
+
+ self._pub = self.create_publisher(JointState, "joint_states", 10)
+
+ self.create_subscription(JointState, "joint_states_body", self._on_body, 10)
+ self.create_subscription(JointState, "joint_states_head", self._on_head, 10)
+
+ def _on_body(self, msg: JointState) -> None:
+ self._update_cache(self._body_cache, msg)
+ self._publish(msg.header)
+
+ def _on_head(self, msg: JointState) -> None:
+ self._update_cache(self._head_cache, msg)
+ self._publish(msg.header)
+
+ @staticmethod
+ def _update_cache(cache: dict, msg: JointState) -> None:
+ positions = list(msg.position) + [0.0] * len(msg.name)
+ velocities = list(msg.velocity) + [0.0] * len(msg.name)
+ efforts = list(msg.effort) + [0.0] * len(msg.name)
+ for i, name in enumerate(msg.name):
+ cache[name] = (positions[i], velocities[i], efforts[i])
+
+ def _publish(self, header) -> None:
+ merged = {**self._body_cache, **self._head_cache}
+ if not merged:
+ return
+
+ msg = JointState()
+ msg.header = header
+ msg.header.stamp = self.get_clock().now().to_msg()
+ for name, (pos, vel, eff) in merged.items():
+ msg.name.append(name)
+ msg.position.append(pos)
+ msg.velocity.append(vel)
+ msg.effort.append(eff)
+
+ self._pub.publish(msg)
+
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = JointStateMergerNode()
+ rclpy.spin(node)
+ node.destroy_node()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/scripts/param_loader_node.py b/scripts/param_loader_node.py
index cbd810b..835352c 100755
--- a/scripts/param_loader_node.py
+++ b/scripts/param_loader_node.py
@@ -23,6 +23,8 @@
# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
+"""Load ROS 2 parameters from YAML and set them on a target node."""
+
import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter
@@ -31,9 +33,23 @@
class ParamLoaderNode(Node):
- """A ROS 2 node that loads nested parameters into another running node."""
+ """Load nested parameters into another running ROS 2 node.
+
+ The node accepts either a YAML file path through ``param_file`` or a YAML
+ string through ``parameters``. Nested YAML dictionaries are flattened into
+ dotted ROS parameter names before being sent to the target node's
+ ``set_parameters`` service.
+
+ Attributes:
+ target_node: Fully qualified name of the node receiving parameters.
+ params: Parsed YAML parameters to send to the target node.
+ client: Client for the target node's ``set_parameters`` service.
+ timer: Timer that retries the service lookup until it is available.
+ attempts: Number of service lookup attempts already made.
+ """
def __init__(self):
+ """Initialize the loader node and parse the configured parameter source."""
super().__init__("param_loader")
self.get_logger().info("🚀ParamLoaderNode started")
@@ -80,7 +96,15 @@ def __init__(self):
self.attempts = 0
def flatten_params(self, prefix, data):
- """Flatten nested parameter dictionaries into ROS-style names."""
+ """Flatten nested parameter dictionaries into ROS-style names.
+
+ Args:
+ prefix: Dotted prefix to prepend to every parameter name.
+ data: Nested dictionary of parameter names and values.
+
+ Returns:
+ A list of ROS parameter messages ready for ``SetParameters``.
+ """
flat = []
for key, value in data.items():
full_name = f"{prefix}.{key}" if prefix else key
@@ -91,6 +115,7 @@ def flatten_params(self, prefix, data):
return flat
def try_set_parameters(self):
+ """Wait for the target service, then send all loaded parameters."""
if not self.client.wait_for_service(timeout_sec=1.0):
self.attempts += 1
if self.attempts > 20:
@@ -113,12 +138,19 @@ def try_set_parameters(self):
self.timer.cancel()
def on_done(self, future):
+ """Handle completion of the asynchronous ``SetParameters`` request.
+
+ Args:
+ future: Future returned by the asynchronous service call.
+ """
try:
result = future.result()
- if result:
+ if result and all(r.successful for r in result.results):
self.get_logger().info(f"Parameters successfully set on {self.target_node}")
else:
- self.get_logger().error(f"Failed to set parameters on {self.target_node}")
+ for i, r in enumerate(result.results):
+ if not r.successful:
+ self.get_logger().error(f"Parameter {i} failed: {r.reason}")
except Exception as e:
self.get_logger().error(f"Exception while setting parameters: {e}")
finally:
@@ -126,6 +158,11 @@ def on_done(self, future):
def main(args=None):
+ """Run the parameter loader node.
+
+ Args:
+ args: Optional ROS command-line arguments.
+ """
rclpy.init(args=args)
node = ParamLoaderNode()
rclpy.spin(node)