Merge pull request #16 from AGH-CEAI/feature/aegis_control_cleanup

macmacal · web-flow · commit 4f18be7e518e · 2025-01-14T12:15:44.000+01:00
aegis_control cleanup &amp; controller_manager config composition
diff --git a/.gitignore b/.gitignore
@@ -3,4 +3,5 @@ logs/
 build/
 bin/
 install/
+__pycache__
 *.sqlite
diff --git a/aegis_bringup/CHANGELOG.md b/aegis_bringup/CHANGELOG.md
@@ -15,6 +15,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 ### Changed
 
 * [PR-9](https://github.com/AGH-CEAI/aegis_ros/pull/9) - Launch and URDF files completely revamped.
+* [PR-16](https://github.com/AGH-CEAI/aegis_ros/pull/16) - Updated launch files diagram.
 
 ### Deprecated
 
diff --git a/aegis_bringup/docs/launch_diagram.plantuml b/aegis_bringup/docs/launch_diagram.plantuml
@@ -24,6 +24,12 @@ package aegis_control {
         Nodes ros2_control::controller_spawners()
     }
 
+    package config {
+        package controllers {
+            class update_rate << (Y,#ffffc9) YAML >> {}
+            class ur_drivers << (Y,#ffffc9) YAML >> {}
+        }
+    }
 }
 
 package aegis_description {
@@ -39,11 +45,9 @@ package aegis_description {
     }
 
     package config {
-        class controllers << (Y,#ffffc9) YAML >> {}
         package ur5e {
             class calibration << (Y,#ffffc9) YAML >> {}
             class joint_limits << (Y,#ffffc9) YAML >> {}
-            class update_rate << (Y,#ffffc9) YAML >> {}
         }
     }
 
@@ -84,12 +88,10 @@ aegis_bringup.bringup *-- aegis_description.robot_description
 aegis_description.robot_description *-- aegis_description.urdf.aegis_urdf
 
 aegis_control.start_drivers *-- aegis_control.ur_driver
-aegis_control.ur_driver *-- aegis_description.config.ur5e.update_rate
-aegis_control.ur_driver *-- aegis_description.config.controllers
+aegis_control.start_drivers *-- aegis_control.config.controllers
 
 aegis_moveit_config.move_group *-- aegis_moveit_config.config
-aegis_moveit_config.move_group *-- aegis_description.config.ur5e.joint_limits
-aegis_moveit_config.move_group *-- aegis_description.config.ur5e.calibration
+aegis_moveit_config.move_group *-- aegis_description.config.ur5e
 
 aegis_description.aegis_preview *-- aegis_description.rviz.preview
 
diff --git a/aegis_bringup/package.xml b/aegis_bringup/package.xml
@@ -16,6 +16,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <exec_depend>aegis_control</exec_depend>
+  <exec_depend>aegis_description</exec_depend>
   <exec_depend>aegis_moveit_config</exec_depend>
   <exec_depend>launch</exec_depend>
   <exec_depend>launch_ros</exec_depend>
diff --git a/aegis_control/CHANGELOG.md b/aegis_control/CHANGELOG.md
@@ -13,6 +13,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* [PR-16](https://github.com/AGH-CEAI/aegis_ros/pull/16) - Smoother controller_manager node config composition.
+
 ### Deprecated
 
 ### Removed
diff --git a/aegis_control/CMakeLists.txt b/aegis_control/CMakeLists.txt
@@ -5,7 +5,7 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 
 install(
-  DIRECTORY launch
+  DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}
 )
 
diff --git a/aegis_control/README.md b/aegis_control/README.md
@@ -1,6 +1,6 @@
 # aegis_control
 
-This package contains the [ros2_control](https://control.ros.org/) launch files for the Aegis robot station, which incorporate configuration files from the `aegis_description` package.
+This package contains the [ros2_control](https://control.ros.org/) launch files for the Aegis robot station.
 
 ## Launch files
 
diff --git a/aegis_control/config/controllers/update_rate.yaml b/aegis_control/config/controllers/update_rate.yaml
diff --git a/aegis_control/config/controllers/ur_drivers.yaml b/aegis_control/config/controllers/ur_drivers.yaml
@@ -1,6 +1,5 @@
 # This file is based on the following source:
 # https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/humble/ur_robot_driver/config/ur_controllers.yaml
-# TODO(issue#10) There might be a better way to integrate multiple controllers (from different drivers)
 # into a single controller_manager  rather than creating one large ROS parameters file.
 
 # Configuration for the ros2_control
@@ -16,7 +15,7 @@
       speed_scaling_state_broadcaster:
         type: ur_controllers/SpeedScalingStateBroadcaster
 
-      force_torque_sensor_broadcaster:
+      ur_force_torque_sensor_broadcaster:
         type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
 
       joint_trajectory_controller:
@@ -59,7 +58,7 @@
     ros__parameters:
       tf_prefix: "$(var tf_prefix)"
 
-  force_torque_sensor_broadcaster:
+  ur_force_torque_sensor_broadcaster:
     ros__parameters:
       sensor_name: $(var tf_prefix)tcp_fts_sensor
       state_interface_names:
diff --git a/aegis_control/launch/include/__init__.py b/aegis_control/launch/include/__init__.py
diff --git a/aegis_control/launch/include/utils.py b/aegis_control/launch/include/utils.py
@@ -0,0 +1,28 @@
+from launch import Condition
+from launch_ros.actions import Node
+
+
+def str2bool(x: str) -> bool:
+    return x.lower() in ("true")
+
+
+def controllers_spawner(
+    controllers: list[str],
+    timeout_s: int = 10,
+    active: bool = True,
+    condition: Condition = None,
+):
+    inactive_flags = ["--inactive"] if not active else []
+    return Node(
+        package="controller_manager",
+        executable="spawner",
+        condition=condition,
+        arguments=[
+            "--controller-manager",
+            "controller_manager",
+            "--controller-manager-timeout",
+            str(timeout_s),
+        ]
+        + inactive_flags
+        + controllers,
+    )
diff --git a/aegis_control/launch/start_drivers.launch.py b/aegis_control/launch/start_drivers.launch.py
@@ -1,10 +1,13 @@
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription
+from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import (
     LaunchConfiguration,
     PathJoinSubstitution,
 )
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile
 from launch_ros.substitutions import FindPackageShare
 
 
@@ -15,7 +18,11 @@ def generate_launch_description() -> LaunchDescription:
         "mock_hardware": LaunchConfiguration("mock_hardware", default="false"),
     }
 
-    # Note: The ur_driver also spawns the control_manager node
+    control_params_files = prepare_params_files()
+    control_nodes = prepare_control_nodes(
+        launch_args["mock_hardware"], control_params_files
+    )
+
     ur_driver = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -25,4 +32,72 @@ def generate_launch_description() -> LaunchDescription:
         launch_arguments=launch_args.items(),
     )
 
-    return LaunchDescription([ur_driver])
+    return LaunchDescription(
+        [
+            ur_driver,
+        ]
+        + control_nodes
+    )
+
+
+def prepare_params_files() -> list[PathJoinSubstitution]:
+    ur_controllers_cfg = ParameterFile(
+        PathJoinSubstitution(
+            [
+                FindPackageShare("aegis_control"),
+                "config",
+                "controllers",
+                "ur_drivers.yaml",
+            ]
+        ),
+        allow_substs=True,
+    )
+    ur_update_rate_config_file = PathJoinSubstitution(
+        [
+            FindPackageShare("aegis_control"),
+            "config",
+            "controllers",
+            "update_rate.yaml",
+        ]
+    )
+
+    return [
+        ur_controllers_cfg,
+        ur_update_rate_config_file,
+    ]
+
+
+def prepare_control_nodes(
+    mock_hardware: LaunchConfiguration, parameters: list[PathJoinSubstitution]
+) -> list[Node]:
+    # The following is due to the design of the ur_robot_driver for ROS 2 Humble
+    return [
+        prepare_fake_control_node(mock_hardware, parameters),
+        prepare_real_control_node(mock_hardware, parameters),
+    ]
+
+
+def prepare_fake_control_node(
+    mock_hardware: LaunchConfiguration, parameters: list[PathJoinSubstitution]
+) -> Node:
+    return Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=parameters,
+        remappings=[("~/robot_description", "robot_description")],
+        output="screen",
+        condition=IfCondition(mock_hardware),
+    )
+
+
+def prepare_real_control_node(
+    mock_hardware: LaunchConfiguration, parameters: list[PathJoinSubstitution]
+) -> Node:
+    return Node(
+        package="ur_robot_driver",
+        executable="ur_ros2_control_node",
+        parameters=parameters,
+        remappings=[("~/robot_description", "robot_description")],
+        output="screen",
+        condition=UnlessCondition(mock_hardware),
+    )
diff --git a/aegis_control/launch/ur_driver.launch.py b/aegis_control/launch/ur_driver.launch.py
@@ -1,18 +1,16 @@
-from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterFile
-from launch_ros.substitutions import FindPackageShare
-
 from launch import LaunchDescription, LaunchContext
 from launch.actions import OpaqueFunction
 from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import (
-    LaunchConfiguration,
-    PathJoinSubstitution,
-)
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
 
+# Bypassing the launch system to access local import
+import os
+import sys
 
-def str2bool(x: str) -> bool:
-    return x.lower() in ("true")
+run_path = os.path.dirname(os.path.realpath(__file__))
+sys.path.append(run_path)
+from include.utils import str2bool, controllers_spawner
 
 
 class URConfig:
@@ -27,35 +25,6 @@ def __init__(self):
         self.real_initial_joint_controller = "scaled_joint_trajectory_controller"
         self.fake_initial_joint_controller = "joint_trajectory_controller"
 
-        # This file requires further substitution with the ParameterFile()
-        self.ur_controllers_cfg = PathJoinSubstitution(
-            [FindPackageShare("aegis_description"), "config", "controllers.yaml"]
-        )
-        self.update_rate_config_file = PathJoinSubstitution(
-            [
-                FindPackageShare("aegis_description"),
-                "config",
-                "ur5e",
-                "update_rate.yaml",
-            ]
-        )
-
-
-def controllers_spawner(controllers: list[str], timeout_s: int = 10, active=True):
-    inactive_flags = ["--inactive"] if not active else []
-    return Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
-            "--controller-manager",
-            "controller_manager",
-            "--controller-manager-timeout",
-            str(timeout_s),
-        ]
-        + inactive_flags
-        + controllers,
-    )
-
 
 def generate_launch_description() -> LaunchDescription:
     return LaunchDescription([OpaqueFunction(function=launch_setup)])
@@ -70,9 +39,6 @@ def launch_setup(context: LaunchContext) -> list[Node]:
 
     cfg = URConfig()
 
-    mock_control_node = prepare_mock_control_node(mock_hardware, cfg)
-    ur_control_node = prepare_ur_control_node(mock_hardware, cfg)
-
     dashboard_client_node = prepare_dashboard_client_node(mock_hardware, cfg)
     tool_communication_node = prepare_tool_communication_node(cfg)
     urscript_interface = prepare_urscript_interface(cfg)
@@ -82,7 +48,7 @@ def launch_setup(context: LaunchContext) -> list[Node]:
         "joint_state_broadcaster",
         "io_and_status_controller",
         "speed_scaling_state_broadcaster",
-        "force_torque_sensor_broadcaster",
+        "ur_force_torque_sensor_broadcaster",
         # "tcp_pose_broadcaster", # TODO(issue#12): debug why this doesn't work
         "ur_configuration_controller",
     ]
@@ -105,50 +71,14 @@ def launch_setup(context: LaunchContext) -> list[Node]:
     controllers_active.append(init_joint_controller)
     controllers_inactive.remove(init_joint_controller)
 
-    controller_spawners = [controllers_spawner(controllers_active)] + [
-        controllers_spawner(controllers_inactive, active=False)
-    ]
-
     return [
-        mock_control_node,
-        ur_control_node,
         dashboard_client_node,
         tool_communication_node,
         controller_stopper_node,
         urscript_interface,
-    ] + controller_spawners
-
-
-def prepare_mock_control_node(
-    mock_hardware: LaunchConfiguration, cfg: URConfig
-) -> Node:
-    return Node(
-        package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[
-            cfg.update_rate_config_file,
-            # implicit usage of the tf_prefix in the cfg.ur_controllers_cfg
-            ParameterFile(cfg.ur_controllers_cfg, allow_substs=True),
-        ],
-        remappings=[("~/robot_description", "robot_description")],
-        output="screen",
-        condition=IfCondition(mock_hardware),
-    )
-
-
-def prepare_ur_control_node(mock_hardware: LaunchConfiguration, cfg: URConfig) -> Node:
-    return Node(
-        package="ur_robot_driver",
-        executable="ur_ros2_control_node",
-        parameters=[
-            cfg.update_rate_config_file,
-            # implicit usage of the tf_prefix in the cfg.ur_controllers_cfg
-            ParameterFile(cfg.ur_controllers_cfg, allow_substs=True),
-        ],
-        remappings=[("~/robot_description", "robot_description")],
-        output="screen",
-        condition=UnlessCondition(mock_hardware),
-    )
+        controllers_spawner(controllers_active),
+        controllers_spawner(controllers_inactive, active=False),
+    ]
 
 
 def prepare_dashboard_client_node(
diff --git a/aegis_control/package.xml b/aegis_control/package.xml
@@ -3,7 +3,7 @@
 <package format="3">
   <name>aegis_control</name>
   <version>0.1.0</version>
-  <description>A package with the ROS 2 control extension of the Aegis robot station description.</description>
+  <description>Contains ROS 2 control launch files for the Aegis robot station.</description>
   <maintainer email="ceai@agh.edu.pl">AGH Center of Excellence in Artificial Intelligence</maintainer>
   <license>Apache-2.0</license>
 
@@ -14,7 +14,6 @@
   <author email="macal@agh.edu.pl">Maciej Aleksandrowicz</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <exec_depend>aegis_description</exec_depend>
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>
   <exec_depend>xacro</exec_depend>

Original file line number	Diff line number	Diff line change
`@@ -5,7 +5,7 @@ find_package(ament_cmake REQUIRED)`
`5`	`5`	`find_package(ament_cmake_python REQUIRED)`
`6`	`6`
`7`	`7`	`install(`
`8`		`- DIRECTORY launch`
	`8`	`+ DIRECTORY config launch`
`9`	`9`	`DESTINATION share/${PROJECT_NAME}`
`10`	`10`	`)`
`11`	`11`