➕ add test MoveitCpp node

rasms_moveit_config/launch/rasms_control.launch.py

@@ -1,9 +1,14 @@
+import os
 from launch.launch_description import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.conditions import UnlessCondition
 from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+from launch.actions import ExecuteProcess
+
+
 
 
 def generate_launch_description():
@@ -54,18 +59,26 @@ def generate_launch_description():
     robot_description = {"robot_description": LaunchConfiguration("robot_description")}
 
     # Load controllers from YAML configuration file
-    controller_configurations = PathJoinSubstitution([
+    """controller_configurations_path = PathJoinSubstitution([
         FindPackageShare(LaunchConfiguration("controller_configurations_package")),
         LaunchConfiguration("controller_configurations")
-    ])
+    ])"""
+    print(robot_description)
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("rasms_moveit_config"),
+        "config",
+        "rasms_controllers.yml",
+    )
 
     # Prepare controller manager and other required nodes
     controller_manager = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[robot_description, controller_configurations],
-        output="screen",
-        condition=UnlessCondition(LaunchConfiguration("sim"))
+        parameters=[robot_description, ros2_controllers_path],
+        output={
+            "stdout": "screen",
+            "stderr": "screen"
+        },
     )
 
     robot_state_publisher = Node(
@@ -75,7 +88,15 @@ def generate_launch_description():
         parameters=[robot_description]
     )
 
-    joint_state_broadcaster = Node(
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base"],
+    )
+
+    """joint_state_broadcaster = Node(
         package="controller_manager",
         executable="spawner.py",
         arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
@@ -84,13 +105,27 @@ def generate_launch_description():
     controller = Node(
         package="controller_manager",
         executable="spawner.py",
-        arguments=[LaunchConfiguration("controller"), "--controller-manager", "/controller_manager"],
-    )
+        arguments=["position_trajectory_controller", "--controller-manager", "/controller_manager"],
+    )"""
+
+    load_controllers = []
+    for controller in [
+        "position_trajectory_controller",
+        "joint_state_broadcaster",
+    ]:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
+                shell=True,
+                output="screen",
+            )
+        ]
 
     return LaunchDescription(
-        launch_args + [
+        launch_args + 
+        [
             controller_manager,
             robot_state_publisher,
-            joint_state_broadcaster,
-            controller
-    ])
+            static_tf
+        ] 
+        + load_controllers)