Update gazebo moveit launchfiles

src/custom_xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py

@@ -63,11 +63,11 @@ def launch_setup(context, *args, **kwargs):
 
     # robot_description
     # xarm_description/launch/lib/robot_description_lib.py
-    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('custom_xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
     get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
     robot_description = {
         'robot_description': get_xacro_file_content(
-            xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), 
+            xacro_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
             arguments={
                 'prefix': prefix,
                 'dof': dof,
@@ -111,7 +111,7 @@ def launch_setup(context, *args, **kwargs):
 
     # gazebo launch
     # gazebo_ros/launch/gazebo.launch.py
-    xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world'])
+    xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'worlds', 'table.world'])
     gazebo_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])),
         launch_arguments={

src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml

@@ -5,30 +5,30 @@ joint_limits:
   joint1:
     has_velocity_limits: true
     max_velocity: 2.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 1.0
   joint2:
     has_velocity_limits: true
     max_velocity: 2.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 1.0
   joint3:
     has_velocity_limits: true
     max_velocity: 2.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 1.0
   joint4:
     has_velocity_limits: true
     max_velocity: 2.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 1.0
   joint5:
     has_velocity_limits: true
     max_velocity: 2.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 1.0
   joint6:
     has_velocity_limits: true
     max_velocity: 2.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 1.0

src/custom_xarm_moveit_config/launch/_robot_moveit_common.launch.py

@@ -52,7 +52,7 @@ def launch_setup(context, *args, **kwargs):
 
     use_sim_time = LaunchConfiguration('use_sim_time', default=False)
 
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
 
     # robot_description_parameters
@@ -60,8 +60,8 @@ def launch_setup(context, *args, **kwargs):
     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')
     robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
         urdf_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns.perform(context).strip('/'),
@@ -106,6 +106,7 @@ def launch_setup(context, *args, **kwargs):
     ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
+    #joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml')
 
     if add_gripper.perform(context) in ('True', 'true'):
         gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
@@ -174,6 +175,31 @@ def launch_setup(context, *args, **kwargs):
         # },
     }
 
+    # FIX acceleration limits
+    for i in range(1,7):
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+
+    #robot_description_parameters['cartesian_limits'] = {}
+    #robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    #robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    #robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    #robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
+
+    pilz_planning_pipeline_config = {
+        'move_group': {
+            'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
+            # Disable AddTimeOptimalParameterization to fix motion blending https://github.com/ros-planning/moveit/issues/2905
+            'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
+            #'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""",
+            "default_planner_config": "PTP",
+        }
+    }
+
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
     # Start the actual move_group node/action server
     move_group_node = Node(
         package='moveit_ros_move_group',
@@ -181,7 +207,9 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
-            ompl_planning_pipeline_config,
+            #ompl_planning_pipeline_config,
+            pilz_planning_pipeline_config,
+            move_group_capabilities,
             trajectory_execution,
             plan_execution,
             moveit_controllers,

src/custom_xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py

@@ -48,7 +48,7 @@ def launch_setup(context, *args, **kwargs):
     # robot moveit common launch
     # xarm_moveit_config/launch/_robot_moveit_common.launch.py
     robot_moveit_common_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -85,7 +85,7 @@ def launch_setup(context, *args, **kwargs):
     # robot gazebo launch
     # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
     robot_gazebo_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,

src/custom_xarm_moveit_config/launch/_robot_moveit_gazebo_fake.launch.py

@@ -0,0 +1,216 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2021, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
+
+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
+
+
+def launch_setup(context, *args, **kwargs):
+    prefix = LaunchConfiguration('prefix', default='')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
+    limited = LaunchConfiguration('limited', default=True)
+    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=7)
+    robot_type = LaunchConfiguration('robot_type', default='xarm')
+    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
+
+    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
+
+    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
+    geometry_type = LaunchConfiguration('geometry_type', default='box')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
+    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
+    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"')
+
+
+    #ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware'
+    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'
+    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
+    
+    # robot description launch
+    # xarm_description/launch/_robot_description.launch.py
+    robot_description_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
+        launch_arguments={
+            '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,
+            'joint_states_remapping': 'joint_states',
+            'add_realsense_d435i': add_realsense_d435i,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            '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,
+        }.items(),
+    )
+
+    # robot moveit common launch
+    # xarm_moveit_config/launch/_robot_moveit_common.launch.py
+    robot_moveit_common_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])),
+        launch_arguments={
+            'prefix': prefix,
+            'hw_ns': hw_ns,
+            'limited': limited,
+            'effort_control': effort_control,
+            'velocity_control': velocity_control,
+            'add_gripper': add_gripper,
+            # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false',
+            'add_vacuum_gripper': add_vacuum_gripper,
+            'dof': dof,
+            'robot_type': robot_type,
+            'no_gui_ctrl': no_gui_ctrl,
+            'ros2_control_plugin': ros2_control_plugin,
+            'controllers_name': controllers_name,
+            'moveit_controller_manager_key': moveit_controller_manager_key,
+            'moveit_controller_manager_value': moveit_controller_manager_value,
+            'add_realsense_d435i': add_realsense_d435i,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            '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,
+        }.items(),
+    )
+
+    remappings = [
+        ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)),
+    ]
+    controllers = ['{}{}_traj_controller'.format(prefix.perform(context), xarm_type)]
+    if add_gripper.perform(context) in ('True', 'true') and robot_type.perform(context) == 'xarm':
+        remappings.append(
+            ('follow_joint_trajectory', '{}xarm_gripper_traj_controller/follow_joint_trajectory'.format(prefix.perform(context)))
+        )
+        controllers.append('{}xarm_gripper_traj_controller'.format(prefix.perform(context)))
+    # 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']}],
+        remappings=remappings,
+    )
+
+    # 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={
+            '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,
+            'add_realsense_d435i': add_realsense_d435i,
+            'add_other_geometry': add_other_geometry,
+            'geometry_type': geometry_type,
+            'geometry_mass': geometry_mass,
+            'geometry_height': geometry_height,
+            'geometry_radius': geometry_radius,
+            '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,
+        }.items(),
+    )
+
+    # Load controllers
+    load_controllers = []
+    for controller in controllers:
+        load_controllers.append(Node(
+            package='controller_manager',
+            executable='spawner',
+            output='screen',
+            arguments=[
+                controller,
+                '--controller-manager', '{}/controller_manager'.format(ros_namespace)
+            ],
+        ))
+
+    # robot gazebo launch
+    # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
+    robot_gazebo_launch = IncludeLaunchDescription(
+        #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
+        launch_arguments={
+            '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,
+            'load_controller': 'true',
+        }.items(),
+    )
+
+    return [
+        robot_description_launch,
+        robot_moveit_common_launch,
+        joint_state_publisher_node,
+        ros2_control_launch,
+        robot_gazebo_launch,
+    ] + load_controllers
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

src/custom_xarm_moveit_config/launch/lite6_moveit_gazebo.launch.py

@@ -40,7 +40,7 @@ def generate_launch_description():
     # robot moveit gazebo launch
     # xarm_moveit_config/launch/_robot_moveit_gazebo.launch.py
     robot_moveit_gazebo_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_gazebo.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'launch', '_robot_moveit_gazebo.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -1,63 +1,47 @@
-#!/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='info')
-    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')
+    hw_ns = LaunchConfiguration('hw_ns', default='ufactory')
-    limited = LaunchConfiguration('limited', default=False)
+    limited = LaunchConfiguration('limited', default=True)
     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')
+    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
-    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='box')
-    geometry_type = LaunchConfiguration('geometry_type', default='cylinder')
+    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
-    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_radius = LaunchConfiguration('geometry_radius', default=0.1)
-    geometry_length = LaunchConfiguration('geometry_length', default=0.07)
+    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
     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)
+    # robot moveit fake launch
-
+    # xarm_moveit_config/launch/_robot_moveit_fake.launch.py
-    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
+    robot_moveit_gazebo_fake_launch = IncludeLaunchDescription(
-
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'launch', '_robot_moveit_gazebo_fake.launch.py'])),
-    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
-    controllers_name = LaunchConfiguration('controllers_name', default='fake_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')
-
-    # robot moveit common launch
-    # xarm_moveit_config/launch/_robot_moveit_common.launch.py
-    robot_moveit_common_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -65,15 +49,11 @@ def launch_setup(context, *args, **kwargs):
             'effort_control': effort_control,
             'velocity_control': velocity_control,
             'add_gripper': add_gripper,
-            # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false',
             'add_vacuum_gripper': add_vacuum_gripper,
-            'dof': dof,
+            'dof': '6',
-            'robot_type': robot_type,
+            'robot_type': 'lite',
             'no_gui_ctrl': 'false',
-            'ros2_control_plugin': ros2_control_plugin,
+            'add_realsense_d435i': add_realsense_d435i,
-            'controllers_name': controllers_name,
-            'moveit_controller_manager_key': moveit_controller_manager_key,
-            'moveit_controller_manager_value': moveit_controller_manager_value,
             'add_other_geometry': add_other_geometry,
             'geometry_type': geometry_type,
             'geometry_mass': geometry_mass,
@@ -86,166 +66,12 @@ def launch_setup(context, *args, **kwargs):
             'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
             'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
             'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
-            'use_sim_time': 'true'
         }.items(),
     )
 
-    # robot gazebo launch
-    # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
-    robot_gazebo_launch = IncludeLaunchDescription(
-        #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
-        launch_arguments={
-            '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,
-            'load_controller': 'true',
-        }.items(),
-    )
-
-    # URDF
-    _robot_description_xml = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']
-            ),
-            #PathJoinSubstitution(
-            #    [
-            #        FindPackageShare('xarm_description'),
-            #        "urdf",
-            #        "lite6",
-            #        #"lite6.urdf.xacro",
-            #        "lite6_robot_macro.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, " ",
-            "geometry_type:=", geometry_type, " ",
-            "geometry_mass:=", geometry_mass, " ",
-            "geometry_height:=", geometry_height, " ",
-            "geometry_radius:=", geometry_radius, " ",
-            "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, " ",
-            #"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('custom_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, " ",
-            #"geometry_type:=", geometry_type, " ",
-            #"geometry_mass:=", geometry_mass, " ",
-            #"geometry_height:=", geometry_height, " ",
-            #"geometry_radius:=", geometry_radius, " ",
-            #"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, " ",
-            #"report_type:=", report_type, " ",
-            #"baud_checkset:=", baud_checkset, " ",
-            #"default_gripper_baud:=", default_gripper_baud, " ",
-        ]
-    )
-    robot_description_semantic = {
-        "robot_description_semantic": _robot_description_semantic_xml
-    }
-
-
-    nodes = [
-        Node(
-            package="lite6_controller",
-            executable="lite6_controller",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[
-                robot_description,
-                robot_description_semantic,
-                {"use_sim_time": use_sim_time},
-            ],
-        ),
-        Node(
-            package="draw_svg",
-            executable="drawing_surface.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-# ######################3
-
-    #moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_fake_controller_manager')
-    #moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_fake_controller_manager/MoveItFakeControllerManager')
-
-    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
-
-
     moveit_config_package_name = 'custom_xarm_moveit_config'
-    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
+    xarm_type = 'lite6'
+    ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware'
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
@@ -262,8 +88,8 @@ def launch_setup(context, *args, **kwargs):
             'velocity_control': velocity_control,
             'add_gripper': add_gripper,
             'add_vacuum_gripper': add_vacuum_gripper,
-            'dof': dof,
+            'dof': '6',
-            'robot_type': robot_type,
+            'robot_type': 'lite',
             'ros2_control_plugin': ros2_control_plugin,
             'add_realsense_d435i': add_realsense_d435i,
             'add_other_geometry': add_other_geometry,
@@ -281,8 +107,8 @@ def launch_setup(context, *args, **kwargs):
         },
         srdf_arguments={
             'prefix': prefix,
-            'dof': dof,
+            'dof': '6',
-            'robot_type': robot_type,
+            'robot_type': 'lite',
             'add_gripper': add_gripper,
             'add_vacuum_gripper': add_vacuum_gripper,
             'add_other_geometry': add_other_geometry,
@@ -293,209 +119,32 @@ def launch_setup(context, *args, **kwargs):
         }
     )
 
-    load_yaml = getattr(mod, 'load_yaml')
+    lite6_controller = Node(
-    controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
+        package="lite6_controller",
-    ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
+        executable="lite6_controller",
-    kinematics_yaml = robot_description_parameters['robot_description_kinematics']
+        output="log",
-    joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
+        #arguments=["--ros-args", "--log-level", log_level],
-
-    # FIX acceleration limits
-    for i in range(1,7):
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
-
-    kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
-    #kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin'
-    #kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
-    kinematics_yaml['kinematics_solver_timeout'] = 10.0
-    kinematics_yaml['kinematics_solver_attempts'] = 10
-
-    #print(joint_limits_yaml)
-    #quit()
-
-    #if add_gripper.perform(context) in ('True', 'true'):
-    #    gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
-    #    gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')
-    #    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:
-    #        for name in gripper_controllers_yaml['controller_names']:
-    #            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))
-
-
-    robot_description_parameters['cartesian_limits'] = {}
-    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
-    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
-    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
-    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
-
-
-    # Planning pipeline
-    # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py
-    #planning_pipeline = {
-    #    "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"],
-    #    "default_planning_pipeline": "pilz_industrial_motion_planner",
-    #    "ompl": {
-    #        "planning_plugin": "ompl_interface/OMPLPlanner",
-    #        # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved
-    #        "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
-    #        # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled
-    #        "start_state_max_bounds_error": 0.31416,
-    #    },
-    #    "pilz_industrial_motion_planner": {
-    #        "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner",
-    #        "default_planner_config": "PTP",
-    #        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
-    #    },
-    #}
-    # Kinematics
-    #kinematics = load_yaml('panda_moveit2_config', 'config/kinematics.yaml')
-
-
-
-
-
-    # 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)
-
-    pilz_planning_pipeline_config = {
-        'move_group': {
-            'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
-            # Disable AddTimeOptimalParameterization to fix motion blending https://github.com/ros-planning/moveit/issues/2905
-            'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            #'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""",
-            "default_planner_config": "PTP",
-        }
-    }
-
-    move_group_capabilities = {
-        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
-    }
-
-
-
-    # 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',
-        arguments=['--log-level', 'debug'],
         parameters=[
             robot_description_parameters,
-            #ompl_planning_pipeline_config,
+            #robot_description,
-            pilz_planning_pipeline_config,
+            #robot_description_semantic,
-            move_group_capabilities,
+            #{"use_sim_time": use_sim_time},
-            trajectory_execution,
-            plan_execution,
-            moveit_controllers,
-            planning_scene_monitor_parameters,
-            {'use_sim_time': use_sim_time},
         ],
     )
 
-    # rviz with moveit configuration
+    virtual_drawing_surface = Node(
-    # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+        package="virtual_drawing_surface",
-    rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+        executable="drawing_surface.py",
-    rviz2_node = Node(
+        output="log",
-        package='rviz2',
+        #arguments=["--ros-args", "--log-level", log_level],
-        executable='rviz2',
+        #parameters=[{"use_sim_time": use_sim_time}],
-        name='rviz2',
-        output='screen',
-        arguments=['-d', rviz_config_file],
-        parameters=[
-            robot_description_parameters,
-            #ompl_planning_pipeline_config,
-            pilz_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'],
-        parameters=[{'use_sim_time': use_sim_time}],
     )
 
     return [
-        #RegisterEventHandler(event_handler=OnProcessExit(
+        robot_moveit_gazebo_fake_launch,
-        #    target_action=rviz2_node,
+        virtual_drawing_surface,
-        #    on_exit=[EmitEvent(event=Shutdown())]
+        lite6_controller,
-        #)),
+    ]
-        #rviz2_node,
-        static_tf,
-        move_group_node,
-        robot_gazebo_launch,
-    ] + nodes
-
-# ######################3
-
-    #return [
-    #    robot_moveit_common_launch,
-    #    robot_gazebo_launch,
-    #] + nodes
 
 
 def generate_launch_description():