From f7fc793b057b5ac154e283b532a4e9a7bf80162b Mon Sep 17 00:00:00 2001 From: mbloechli Date: Sat, 30 May 2026 17:24:38 +0200 Subject: [PATCH 1/2] feat(): added controller activator to activate all controller at the same time, added hardware activator to activate hardware only when controllers are ready to activate, moved controller_manager node into duatic_control since it needs to know if hardware components should be set to active or not on startup --- .gitignore | 1 + CMakeLists.txt | 2 + README.md | 148 +++++++++++++++----- launch/control.launch.py | 84 ++++++++++-- package.xml | 1 + scripts/controller_activator_node.py | 141 +++++++++++++++++++ scripts/hardware_activator_node.py | 198 +++++++++++++++++++++++++++ scripts/param_loader_node.py | 45 +++++- 8 files changed, 568 insertions(+), 52 deletions(-) create mode 100644 scripts/controller_activator_node.py create mode 100644 scripts/hardware_activator_node.py 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..12e92da 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,8 @@ install(DIRECTORY install( PROGRAMS scripts/param_loader_node.py + scripts/hardware_activator_node.py + scripts/controller_activator_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 @@ [![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 - + ``` +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/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) From 929acf6005f8e48408c906ab37708cc44f3bda87 Mon Sep 17 00:00:00 2001 From: svenkae1234 Date: Wed, 3 Jun 2026 16:12:56 +0200 Subject: [PATCH 2/2] added joint_state merger node --- CMakeLists.txt | 1 + scripts/joint_state_merger_node.py | 86 ++++++++++++++++++++++++++++++ 2 files changed, 87 insertions(+) create mode 100644 scripts/joint_state_merger_node.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 12e92da..d716ee4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ install( 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/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()