Implement working real robot launch

src/draw_svg/launch/lite6_real.launch.py

@@ -1,58 +1,20 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch Python example for following a target"""
-
 import os
+from launch import LaunchDescription
+from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
 from ament_index_python import get_package_share_directory
 from launch.launch_description_sources import load_python_launch_file_as_module
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
+from launch.actions import RegisterEventHandler, EmitEvent
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, FindExecutable
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 from launch.event_handlers import OnProcessExit
-from launch.actions import OpaqueFunction
+from launch.events import Shutdown
 
 
 def launch_setup(context, *args, **kwargs):
-    use_sim_time = LaunchConfiguration("use_sim_time", default=True)
-    log_level = LaunchConfiguration("log_level", default='warn')
-    rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz"))
-
-    prefix = LaunchConfiguration('prefix', default='')
-    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
-    limited = LaunchConfiguration('limited', default=False)
-    effort_control = LaunchConfiguration('effort_control', default=False)
-    velocity_control = LaunchConfiguration('velocity_control', default=False)
-    add_gripper = LaunchConfiguration('add_gripper', default=False)
-    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
-    dof = LaunchConfiguration('dof', default=6)
-    robot_type = LaunchConfiguration('robot_type', default='lite')
-    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
-
-    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
-    #geometry_type = LaunchConfiguration('geometry_type', default='box')
-    geometry_type = LaunchConfiguration('geometry_type', default='cylinder')
-    geometry_mass = LaunchConfiguration('geometry_mass', default=0.05)
-    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
-    geometry_radius = LaunchConfiguration('geometry_radius', default=0.005)
-    geometry_length = LaunchConfiguration('geometry_length', default=0.07)
-    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
-    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
-    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
-    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
-    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
-    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
-    load_controller = LaunchConfiguration('load_controller', default=True)
-
-    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
-
-
-    ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem'
-    controllers_name = 'fake_controllers'
-    moveit_controller_manager_key = 'moveit_simple_controller_manager'
-    moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
-
     robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
     report_type = LaunchConfiguration('report_type', default='normal')
     prefix = LaunchConfiguration('prefix', default='')
@@ -62,6 +24,9 @@ def launch_setup(context, *args, **kwargs):
     velocity_control = LaunchConfiguration('velocity_control', default=False)
     add_gripper = LaunchConfiguration('add_gripper', default=False)
     add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=6)
+    robot_type = LaunchConfiguration('robot_type', default='lite')
+    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
 
     add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
     geometry_type = LaunchConfiguration('geometry_type', default='box')
@@ -76,13 +41,42 @@ def launch_setup(context, *args, **kwargs):
     geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
     geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
 
-    # robot moveit realmove launch
+    baud_checkset = LaunchConfiguration('baud_checkset', default=True)
-    # xarm_moveit_config/launch/_robot_moveit_realmove.launch.py
+    default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000)
-    robot_moveit_realmove_launch = IncludeLaunchDescription(
+
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_realmove.launch.py'])),
+    ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware'
+    controllers_name = LaunchConfiguration('controllers_name', default='controllers')
+    moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
+    moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
+
+    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default=False)
+    log_level = LaunchConfiguration("log_level", default='warn')
+
+    # # robot driver launch
+    # # xarm_api/launch/_robot_driver.launch.py
+    # robot_driver_launch = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])),
+    #     launch_arguments={
+    #         'robot_ip': robot_ip,
+    #         'report_type': report_type,
+    #         'dof': dof,
+    #         'hw_ns': hw_ns,
+    #         'add_gripper': add_gripper,
+    #         'prefix': prefix,
+    #         'baud_checkset': baud_checkset,
+    #         'default_gripper_baud': default_gripper_baud,
+    #         'robot_type': robot_type,
+    #     }.items(),
+    # )
+
+    # robot description launch
+    # xarm_description/launch/_robot_description.launch.py
+    robot_description_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
-            'robot_ip': robot_ip,
-            'report_type': report_type,
             'prefix': prefix,
             'hw_ns': hw_ns,
             'limited': limited,
@@ -90,9 +84,10 @@ def launch_setup(context, *args, **kwargs):
             'velocity_control': velocity_control,
             'add_gripper': add_gripper,
             'add_vacuum_gripper': add_vacuum_gripper,
-            'dof': '6',
+            'dof': dof,
-            'robot_type': 'lite',
+            'robot_type': robot_type,
-            'no_gui_ctrl': 'false',
+            'ros2_control_plugin': ros2_control_plugin,
+            'joint_states_remapping': PathJoinSubstitution(['/', ros_namespace, hw_ns, 'joint_states']),
             'add_other_geometry': add_other_geometry,
             'geometry_type': geometry_type,
             'geometry_mass': geometry_mass,
@@ -108,110 +103,214 @@ def launch_setup(context, *args, **kwargs):
         }.items(),
     )
 
-    # URDF
+    # robot_description_parameters
-    _robot_description_xml = Command(
+    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-        [
+    moveit_config_package_name = 'xarm_moveit_config'
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
-            " ",
+    get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
-            PathJoinSubstitution(
+    robot_description_parameters = get_xarm_robot_description_parameters(
-                [FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-            ),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
-            #PathJoinSubstitution(
+        urdf_arguments={
-            #    [
+            'prefix': prefix,
-            #        FindPackageShare('xarm_description'),
+            'hw_ns': hw_ns.perform(context).strip('/'),
-            #        "urdf",
+            'limited': limited,
-            #        "lite6",
+            'effort_control': effort_control,
-            #        #"lite6.urdf.xacro",
+            'velocity_control': velocity_control,
-            #        "lite6_robot_macro.xacro",
+            'add_gripper': add_gripper,
-            #    ]
+            'add_vacuum_gripper': add_vacuum_gripper,
-            #),
+            'dof': dof,
-            " ",
+            'robot_type': robot_type,
-            #"name:=", "lite6", " ",
+            'ros2_control_plugin': ros2_control_plugin,
-            "prefix:=", " ",
+            'add_other_geometry': add_other_geometry,
-            "hw_ns:=", hw_ns, " ",
+            'geometry_type': geometry_type,
-            "limited:=", limited, " ",
+            'geometry_mass': geometry_mass,
-            "effort_control:=", effort_control, " ",
+            'geometry_height': geometry_height,
-            "velocity_control:=", velocity_control, " ",
+            'geometry_radius': geometry_radius,
-            "add_gripper:=", add_gripper, " ",
+            'geometry_length': geometry_length,
-            "add_vacuum_gripper:=", add_vacuum_gripper, " ",
+            'geometry_width': geometry_width,
-            "dof:=", dof, " ",
+            'geometry_mesh_filename': geometry_mesh_filename,
-            "robot_type:=", robot_type, " ",
+            'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
-            "ros2_control_plugin:=", ros2_control_plugin, " ",
+            'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
-            #"ros2_control_params:=", ros2_control_params, " ",
+            'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
-
+            'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
-            "add_other_geometry:=", add_other_geometry, " ",
+        },
-            "geometry_type:=", geometry_type, " ",
+        srdf_arguments={
-            "geometry_mass:=", geometry_mass, " ",
+            'prefix': prefix,
-            "geometry_height:=", geometry_height, " ",
+            'dof': dof,
-            "geometry_radius:=", geometry_radius, " ",
+            'robot_type': robot_type,
-            "geometry_length:=", geometry_length, " ",
+            'add_gripper': add_gripper,
-            "geometry_width:=", geometry_width, " ",
+            'add_vacuum_gripper': add_vacuum_gripper,
-            "geometry_mesh_filename:=", geometry_mesh_filename, " ",
+            'add_other_geometry': add_other_geometry,
-            "geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
+        },
-            "geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
+        arguments={
-            "geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
+            'context': context,
-            "geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
+            'xarm_type': xarm_type,
-
+        }
-            #"robot_ip:=", robot_ip, " ",
-            #"report_type:=", report_type, " ",
-            #"baud_checkset:=", baud_checkset, " ",
-            #"default_gripper_baud:=", default_gripper_baud, " ",
-        ]
     )
-    # TODO fix URDF loading
-    # xacro urdf/xarm_pen.urdf.xacro prefix:= hw_ns:=xarm dof:=6 limited:=false effort_control:=false velocity_control:=false add_gripper:=false add_vacuum_gripper:=false robot_type:=lite ros2_control_plugin:=gazebo_ros2_control/GazeboSystem add_other_geometry:=false geometry_type:=cylinder geometry_mass:=0.05 geometry_height:=0.1 geometry_radius:=0.005 geometry_length:=0.07 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:="0 0 0" geometry_mesh_origin_rpy:="0 0 0" geometry_mesh_tcp_xyz:="0 0 0" geometry_mesh_tcp_rpy:="0 0 0"
-    _robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])])
-    robot_description = {"robot_description": _robot_description_xml}
-    # SRDF
-    _robot_description_semantic_xml = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare('xarm_moveit_config'),
-                    "srdf",
-                    #"_lite6_macro.srdf.xacro",
-                    "xarm.srdf.xacro",
-                ]
-            ),
-            " ",
-            #"name:=", "lite6", " ",
-            "prefix:=", " ",
-            #"hw_ns:=", hw_ns, " ",
-            #"limited:=", limited, " ",
-            #"effort_control:=", effort_control, " ",
-            #"velocity_control:=", velocity_control, " ",
-            #"add_gripper:=", add_gripper, " ",
-            #"add_vacuum_gripper:=", add_vacuum_gripper, " ",
-            "dof:=", dof, " ",
-            "robot_type:=", robot_type, " ",
-            #"ros2_control_plugin:=", ros2_control_plugin, " ",
-            #"ros2_control_params:=", ros2_control_params, " ",
 
-            #"add_other_geometry:=", add_other_geometry, " ",
+    load_yaml = getattr(mod, 'load_yaml')
-            #"geometry_type:=", geometry_type, " ",
+    controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
-            #"geometry_mass:=", geometry_mass, " ",
+    ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
-            #"geometry_height:=", geometry_height, " ",
+    kinematics_yaml = robot_description_parameters['robot_description_kinematics']
-            #"geometry_radius:=", geometry_radius, " ",
+    joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
-            #"geometry_length:=", geometry_length, " ",
-            #"geometry_width:=", geometry_width, " ",
-            #"geometry_mesh_filename:=", geometry_mesh_filename, " ",
-            #"geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
-            #"geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
-            #"geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
-            #"geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
 
-            #"robot_ip:=", robot_ip, " ",
+    if add_gripper.perform(context) in ('True', 'true'):
-            #"report_type:=", report_type, " ",
+        gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
-            #"baud_checkset:=", baud_checkset, " ",
+        gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')
-            #"default_gripper_baud:=", default_gripper_baud, " ",
+        gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml')
-        ]
+
-    )
+        if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml:
-    robot_description_semantic = {
+            for name in gripper_controllers_yaml['controller_names']:
-        "robot_description_semantic": _robot_description_semantic_xml
+                if name in gripper_controllers_yaml:
+                    if name not in controllers_yaml['controller_names']:
+                        controllers_yaml['controller_names'].append(name)
+                    controllers_yaml[name] = gripper_controllers_yaml[name]
+        if gripper_ompl_planning_yaml:
+            ompl_planning_yaml.update(gripper_ompl_planning_yaml)
+        if joint_limits_yaml and gripper_joint_limits_yaml:
+            joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
+
+    add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
+    add_prefix_to_moveit_params(
+        controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml,
+        kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
+        prefix=prefix.perform(context))
+
+    # Planning Configuration
+    ompl_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'ompl_interface/OMPLPlanner',
+            'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
+            'start_state_max_bounds_error': 0.1,
+        }
+    }
+    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
+
+    # Moveit controllers Configuration
+    moveit_controllers = {
+        moveit_controller_manager_key.perform(context): controllers_yaml,
+        'moveit_controller_manager': moveit_controller_manager_value.perform(context),
     }
 
+    # Trajectory Execution Configuration
+    trajectory_execution = {
+        'moveit_manage_controllers': True,
+        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
+        'trajectory_execution.allowed_goal_duration_margin': 0.5,
+        'trajectory_execution.allowed_start_tolerance': 0.01,
+        'trajectory_execution.execution_duration_monitoring': False
+    }
+
+    plan_execution = {
+        'plan_execution.record_trajectory_state_frequency': 10.0,
+    }
+
+    planning_scene_monitor_parameters = {
+        'publish_planning_scene': True,
+        'publish_geometry_updates': True,
+        'publish_state_updates': True,
+        'publish_transforms_updates': True,
+        # "planning_scene_monitor_options": {
+        #     "name": "planning_scene_monitor",
+        #     "robot_description": "robot_description",
+        #     "joint_state_topic": "/joint_states",
+        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
+        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
+        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
+        #     "wait_for_initial_state_timeout": 10.0,
+        # },
+    }
+
+    # Start the actual move_group node/action server
+    move_group_node = Node(
+        package='moveit_ros_move_group',
+        executable='move_group',
+        output='screen',
+        parameters=[
+            robot_description_parameters,
+            ompl_planning_pipeline_config,
+            trajectory_execution,
+            plan_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {'use_sim_time': use_sim_time},
+        ],
+    )
+
+    # rviz with moveit configuration
+    # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+    rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+    rviz2_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz_config_file],
+        parameters=[
+            robot_description_parameters,
+            ompl_planning_pipeline_config,
+            {'use_sim_time': use_sim_time},
+        ],
+        remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+
+    # Static TF
+    static_tf = Node(
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        name='static_transform_publisher',
+        output='screen',
+        arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link_base'],
+    )
+
+
+    # joint state publisher node
+    joint_state_publisher_node = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        output='screen',
+        parameters=[{'source_list': ['{}/joint_states'.format(hw_ns.perform(context))]}],
+        remappings=[
+            ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)),
+        ],
+    )
+
+    # ros2 control launch
+    # xarm_controller/launch/_ros2_control.launch.py
+    ros2_control_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_ros2_control.launch.py'])),
+        launch_arguments={
+            'robot_ip': robot_ip,
+            'report_type': report_type,
+            'baud_checkset': baud_checkset,
+            'default_gripper_baud': default_gripper_baud,
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'ros2_control_plugin': ros2_control_plugin,
+        }.items(),
+    )
+
+    control_node = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        arguments=[
+            '{}{}_traj_controller'.format(prefix.perform(context), xarm_type),
+            '--controller-manager', '{}/controller_manager'.format(ros_namespace)
+        ],
+    )
 
     nodes = [
         Node(
@@ -220,8 +319,11 @@ def launch_setup(context, *args, **kwargs):
             output="log",
             arguments=["--ros-args", "--log-level", log_level],
             parameters=[
-                robot_description,
+                #robot_description_parameters['robot_description'],
-                robot_description_semantic,
+                #robot_description_parameters['robot_description_semantic'],
+                #robot_description_parameters['robot_description_planning'],
+                #robot_description_parameters['robot_description_kinematics'],
+                robot_description_parameters,
                 {"use_sim_time": use_sim_time},
             ],
         ),
@@ -235,7 +337,21 @@ def launch_setup(context, *args, **kwargs):
     ]
 
     return [
-        robot_moveit_realmove_launch,
+
+        RegisterEventHandler(event_handler=OnProcessExit(
+            target_action=rviz2_node,
+            on_exit=[EmitEvent(event=Shutdown())]
+        )),
+        rviz2_node,
+        static_tf,
+        move_group_node,
+
+
+        robot_description_launch,
+        joint_state_publisher_node,
+        ros2_control_launch,
+        control_node,
+        # robot_driver_launch,
     ] + nodes