Update readme

README.md

@@ -29,26 +29,18 @@ docker builder prune --all --force
 bash .docker/run.bash
 ```
 
+### Develop 
 If active changes are being made, run:
 ``` sh
 bash .docker/devel.bash
 ```
 This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
 
-## TODO Building locally
+So to test changes in the container
-
-Requirements:
-- python3-pip 
-- python3-pil.imagetk
-- ros-humble-moveit
-- ros-humble-ros-gz
-- ignition-fortress
-
 ``` sh
-./rebuild.sh
+cd src/drawing-robot-ros2/src
-```
+colcon build
-``` sh
+source install/local_setup.bash
-source src/install/local_setup.bash
 ```
 
 ## Running

src/custom_xarm_description/urdf/lite6/lite6.urdf.xacro

@@ -363,5 +363,50 @@
       <child
         link="${prefix}link_eef" />
     </joint> -->
+
+
+    <link name="${prefix}pen_link">
+    <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+            <cylinder radius="0.005" length="0.2"/>
+        </geometry>
+        <material name="Cyan">
+            <color rgba="0.0 1.0 1.0 1.0"/>
+        </material>
+    </visual>
+
+    <collision>
+        <geometry>
+            <box size="0 0 0"/>
+        </geometry>
+    </collision>
+    </link>
+
+    <joint name="${prefix}pen_joint" type="fixed">
+      <parent link="${prefix}link6"/>
+      <child link="${prefix}pen_link"/>
+    </joint>
+
+  <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
+  <gazebo>
+    <plugin name="pen_position" filename="libgazebo_ros_p3d.so">
+        <!-- <alwaysOn>true</alwaysOn> -->
+      <ros>
+        <remapping>odom:=pen_position</remapping>
+      </ros>
+      <frame_name>world</frame_name>
+      <!-- <body_name>pen_link</body_name> -->
+      <body_name>${prefix}link6</body_name>
+      <!-- <body_name>${prefix}pen_link</body_name> -->
+      <!--       topic name is always /odom -->
+      <!-- <topic_name>pen_position</topic_name> -->
+      <!--       Update rate in Hz -->
+      <update_rate>1000</update_rate>
+      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
+      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
+    </plugin>
+  </gazebo>
+
   </xacro:macro>
-</robot>
+</robot>

src/custom_xarm_description/urdf/lite6/lite6_robot_macro.xacro

@@ -6,14 +6,14 @@
     robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000 ">
 
     <!-- include lite6 relative macros: -->
-    <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.ros2_control.xacro" />
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.ros2_control.xacro" />
-    <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro" />
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.urdf.xacro" />
-    <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.transmission.xacro" />
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.transmission.xacro" />
-    <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.gazebo.xacro" />
+    <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.gazebo.xacro" />
 
     <!-- gazebo_plugin -->
     <xacro:if value="${load_gazebo_plugin}">
-      <xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
+      <xacro:include filename="$(find custom_xarm_description)/urdf/common/common.gazebo.xacro" />
       <xacro:gazebo_ros_control_plugin  prefix="${prefix}" ros2_control_params="${ros2_control_params}"/>
     </xacro:if>
 

src/custom_xarm_description/urdf/xarm_device.urdf.xacro

@@ -33,11 +33,11 @@
   <xacro:arg name="default_gripper_baud" default="2000000"/>
 
   <!-- gazebo_plugin -->
-  <xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
+  <xacro:include filename="$(find custom_xarm_description)/urdf/common/common.gazebo.xacro" />
   <xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/>
   
   <!-- load xarm device -->
-  <xacro:include filename="$(find xarm_description)/urdf/xarm_device_macro.xacro" />
+  <xacro:include filename="$(find custom_xarm_description)/urdf/xarm_device_macro.xacro" />
   <xacro:xarm_device prefix="$(arg prefix)" namespace="$(arg hw_ns)" limited="$(arg limited)" 
     effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
     add_gripper="$(arg add_gripper)" add_vacuum_gripper="$(arg add_vacuum_gripper)" dof="$(arg dof)" 

src/custom_xarm_description/urdf/xarm_device_macro.xacro

@@ -35,7 +35,7 @@
     <xacro:if value="${dof == 6}">
       <xacro:if value="${robot_type == 'lite'}">
         <!-- Load Lite6 Robot Model URDF -->
-        <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6_robot_macro.xacro" />
+        <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6_robot_macro.xacro" />
         <xacro:lite6_robot prefix="${prefix}" namespace="${namespace}" limited="${limited}" 
           effort_control="${effort_control}" velocity_control="${velocity_control}" 
           ros2_control_plugin="${ros2_control_plugin}" 
@@ -137,4 +137,4 @@
     </xacro:if>
 
   </xacro:macro>
-</robot>
+</robot>

src/custom_xarm_gazebo/launch/lite6_virtual_surface.launch.py

@@ -0,0 +1,177 @@
+import os
+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.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.event_handlers import OnProcessExit
+from launch.actions import OpaqueFunction
+
+
+def launch_setup(context, *args, **kwargs):
+    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=7)
+    robot_type = LaunchConfiguration('robot_type', default='xarm')
+    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_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"')
+    load_controller = LaunchConfiguration('load_controller', default=False)
+
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
+
+    # ros2 control params
+    # xarm_controller/launch/lib/robot_controller_lib.py
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py'))
+    generate_ros2_control_params_temp_file = getattr(mod, 'generate_ros2_control_params_temp_file')
+    ros2_control_params = generate_ros2_control_params_temp_file(
+        os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type.perform(context), dof.perform(context))),
+        prefix=prefix.perform(context),
+        add_gripper=add_gripper.perform(context) in ('True', 'true'),
+        ros_namespace=LaunchConfiguration('ros_namespace', default='').perform(context),
+        update_rate=1000,
+        #robot_type=robot_type.perform(context)
+    )
+
+    # robot_description
+    # 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'))
+    #mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('draw_svg'), 'launch', 'robots', 'xarm_with_pen.py'))
+    get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
+    robot_description = {
+        'robot_description': get_xacro_file_content(
+            xacro_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
+            #xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']),
+            arguments={
+                'prefix': prefix,
+                'dof': dof,
+                'robot_type': robot_type,
+                'add_gripper': add_gripper,
+                'add_vacuum_gripper': add_vacuum_gripper,
+                'hw_ns': hw_ns.perform(context).strip('/'),
+                'limited': limited,
+                'effort_control': effort_control,
+                'velocity_control': velocity_control,
+                '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 state publisher node
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[robot_description],
+        remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+
+    # gazebo launch
+    # gazebo_ros/launch/gazebo.launch.py
+    #xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world'])
+    #xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('draw_svg'), 'worlds', 'draw_svg_world.sdf'])
+    xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'worlds', 'virtual_surface_world.sdf'])
+    #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={
+            'world': xarm_gazebo_world,
+            'server_required': 'true',
+            'gui_required': 'true',
+        }.items(),
+    )
+
+    # gazebo spawn entity node
+    gazebo_spawn_entity_node = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        output='screen',
+        arguments=[
+            '-topic', 'robot_description',
+            '-entity', '{}{}'.format(robot_type.perform(context), dof.perform(context)),
+            '-x', '-0.2',
+            #'-x', '0.0',
+            '-y', '-0.5',
+            #'-y', '0.0',
+            '-z', '1.021',
+            #'-z', '0.0',
+            '-Y', '1.571',
+            #'-Y', '0.0',
+        ],
+    )
+
+    # Load controllers
+    controllers = [
+        'joint_state_broadcaster',
+        '{}{}{}_traj_controller'.format(prefix.perform(context), robot_type.perform(context), dof.perform(context)),
+    ]
+    if robot_type.perform(context) == 'xarm' and add_gripper.perform(context) in ('True', 'true'):
+        controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context)))
+    load_controllers = []
+    if load_controller.perform(context) in ('True', 'true'):
+        for controller in controllers:
+            #print("Attempting to load: ", controller)
+            #input()
+            load_controllers.append(Node(
+                package='controller_manager',
+                #executable='spawner.py',
+                executable='spawner',
+                output='screen',
+                arguments=[
+                    controller,
+                    '--controller-manager', '{}/controller_manager'.format(ros_namespace)
+                ],
+            ))
+
+    return [
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=gazebo_spawn_entity_node,
+                on_exit=load_controllers,
+            )
+        ),
+        gazebo_launch,
+        robot_state_publisher_node,
+        gazebo_spawn_entity_node,
+    ]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

src/custom_xarm_gazebo/worlds/virtual_surface_world.sdf

@@ -0,0 +1,187 @@
+<?xml version="1.0"?>
+<!-- <sdf version="1.9"> -->
+<!-- <sdf version="1.7"> -->
+<sdf version="1.7">
+  <world name="virtual_surface_world">
+
+    <!-- Physics -->
+    <!-- <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
+         <engine>
+         <filename>ignition-physics-dartsim-plugin</filename>
+         </engine>
+         <dart>
+         <collision_detector>bullet</collision_detector>
+         </dart>
+         </plugin>
+         <physics name="physics_config" type="dart">
+         <max_step_size>0.005</max_step_size>
+         <real_time_factor>1.0</real_time_factor>
+         </physics>
+    -->
+    <physics type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+      <!-- (2022-12-20) humble: change gravity from [0 0 -9.81] to [0 0 0] -->
+      <gravity>0 0 0</gravity>
+    </physics>
+
+    <!-- Scene -->
+    <!-- <plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
+         </plugin>
+         <scene>
+         <ambient>0.8 0.8 0.8</ambient>
+         <grid>false</grid>
+         </scene>
+    -->
+    <!-- User Commands (transform control) -->
+    <!-- <plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
+         </plugin>
+    -->
+
+    <!--              -->
+    <!-- Illumination -->
+    <!--              -->
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.3 0.3 -0.9</direction>
+    </light>
+
+
+    <!--        -->
+    <!-- Models -->
+    <!--        -->
+    <!-- Ground -->
+    <model name="ground_plane">
+      <pose>0 0 0 0 0 0</pose>
+      <static>true</static>
+      <link name="ground_plane_link">
+      <collision name="ground_plane_collision">
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+          </plane>
+        </geometry>
+      </collision>
+      <visual name="ground_plane_visual">
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>4 4</size>
+          </plane>
+        </geometry>
+        <material>
+          <diffuse>0.2 0.2 0.2 1</diffuse>
+          <specular>0.2 0.2 0.2 1</specular>
+        </material>
+      </visual>
+         </link>
+    </model>
+    <model name="drawing_surface">
+      <pose>-0.1485 -0.3 0.5 0 0 0</pose>
+      <static>true</static>
+      <link name="drawing_surface_link">
+        <collision name="drawing_surface_collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>0.297 0.21</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="drawing_surface_visual">
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+             <plane>
+               <normal>0 0 1</normal>
+               <size>0.297 0.21</size>
+             </plane>
+           </geometry>
+      <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_video.cpp -->
+      <!--  https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/worlds/gazebo_ros_video_demo.world -->
+        <!--         TODO: note docs outdated: https://classic.gazebosim.org/tutorials?tut=ros_gzplugins -->
+          <plugin name="drawing_surface" filename="libgazebo_ros_video.so">
+            <!-- <ros>
+                 <remapping>~/image_raw:=/camera1/image_raw</remapping>
+                 </ros> -->
+            <!-- 300dpi A4 paper is 2480x3508 px -->
+            <!-- <height>2480</height>
+                 <width>3508</width> -->
+            <!-- 200dpi A4 paper is 1654x2339 px -->
+            <height>1654</height>
+            <width>2339</width>
+          </plugin>
+        </visual>
+        <!-- <inertial>
+             <origin xyz="0 0 1" rpy="0 0 0"/>
+             <mass value="1"/>
+             <inertia
+             ixx="1.0" ixy="0.0" ixz="0.0"
+             iyy="1.0" iyz="0.0"
+             izz="1.0"/>
+             </inertial> -->
+      </link>
+    </model>
+    <!-- <include>
+         <uri>model://ground_plane</uri>
+         <pose>0.0 -0.84 0 0 0 0</pose>
+         </include> -->
+    <!-- <include>
+         <uri>model://sun</uri>
+         </include> -->
+    <!-- <include>
+         <uri>model://table</uri>
+         <name>table</name>
+         <pose>0.0 -0.84 0 0 0 0</pose>
+         </include>
+    -->
+    <!-- Static target -->
+    <!-- <model name="target">
+         <static>true</static>
+         <pose>0.5 -0.25 0.5 3.1415927 0 0</pose>
+         <link name="target_link">
+         <visual name="target_visual">
+         <cast_shadows>false</cast_shadows>
+         <geometry>
+         <box>
+         <size>0.04 0.04 0.04</size>
+         </box>
+         </geometry>
+         <material>
+         <diffuse>0.1 0.1 0.1 1</diffuse>
+         <specular>0.4 0.4 0.4 1</specular>
+         </material>
+         </visual>
+         </link>
+    -->
+      <!-- Pose publisher plugin, used to determine the target pose to reach -->
+    <!-- <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
+         <publish_nested_model_pose>true</publish_nested_model_pose>
+         <publish_link_pose>false</publish_link_pose>
+         <publish_collision_pose>false</publish_collision_pose>
+         <publish_visual_pose>false</publish_visual_pose>
+         </plugin>
+         </model> -->
+    <!-- <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
+         <publish_link_pose>true</publish_link_pose>
+         <publish_collision_pose>false</publish_collision_pose>
+         <publish_visual_pose>false</publish_visual_pose>
+         <publish_nested_model_pose>false</publish_nested_model_pose>
+         </plugin> -->
+
+    <!-- <include>
+         <uri>model://table</uri>
+         <name>table</name>
+         <pose>0.0 -0.84 0 0 0 0</pose>
+         </include> -->
+  </world>
+</sdf>

src/custom_xarm_moveit_config/launch/_robot_moveit_gazebo_fake.launch.py

@@ -13,6 +13,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
 from launch_ros.actions import Node
 
+from launch.actions import TimerAction
 
 def launch_setup(context, *args, **kwargs):
     prefix = LaunchConfiguration('prefix', default='')
@@ -184,8 +185,8 @@ 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('custom_xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
+        #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -198,6 +199,7 @@ def launch_setup(context, *args, **kwargs):
             'robot_type': robot_type,
             'ros2_control_plugin': ros2_control_plugin,
             'load_controller': 'true',
+            #'load_controller': 'false',
         }.items(),
     )
 
@@ -206,7 +208,10 @@ def launch_setup(context, *args, **kwargs):
         robot_moveit_common_launch,
         joint_state_publisher_node,
         ros2_control_launch,
-        robot_gazebo_launch,
+        # Wait before launching gazebo
+        TimerAction(period=5.0, actions=[
+            robot_gazebo_launch,
+        ]),
     ] + load_controllers
 
 

src/custom_xarm_moveit_config/srdf/xarm.srdf.xacro

@@ -8,8 +8,8 @@
   <xacro:arg name="add_vacuum_gripper" default="false" />
   <xacro:arg name="add_other_geometry" default="false" />
 
-  <xacro:include filename="$(find xarm_moveit_config)/srdf/xarm_macro.srdf.xacro" />
+  <xacro:include filename="$(find custom_xarm_moveit_config)/srdf/xarm_macro.srdf.xacro" />
   <xacro:xarm_macro_srdf prefix="$(arg prefix)" dof="$(arg dof)" robot_type="$(arg robot_type)" 
     add_gripper="$(arg add_gripper)" add_vacuum_gripper="$(arg add_vacuum_gripper)" add_other_geometry="$(arg add_other_geometry)" />
 
-</robot>
+</robot>

src/custom_xarm_moveit_config/srdf/xarm_macro.srdf.xacro

@@ -9,7 +9,7 @@
     </xacro:if>
     <xacro:if value="${dof == 6}">
       <xacro:if value="${robot_type == 'lite'}">
-        <xacro:include filename="$(find xarm_moveit_config)/srdf/_lite6_macro.srdf.xacro" />
+        <xacro:include filename="$(find custom_xarm_moveit_config)/srdf/_lite6_macro.srdf.xacro" />
         <xacro:lite6_macro_srdf prefix="${prefix}" 
           add_gripper="${add_gripper}" add_vacuum_gripper="${add_vacuum_gripper}" add_other_geometry="${add_other_geometry}"  />
       </xacro:if>
@@ -25,4 +25,4 @@
         add_gripper="${add_gripper}" add_vacuum_gripper="${add_vacuum_gripper}" add_other_geometry="${add_other_geometry}"  />
     </xacro:unless>
   </xacro:macro>
-</robot>
+</robot>

src/draw_svg/launch/robots/lite6_table.launch.py

@@ -54,12 +54,12 @@ 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'))
     #mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('draw_svg'), 'launch', 'robots', 'xarm_with_pen.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']),
             #xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']),
             arguments={
                 'prefix': prefix,
@@ -105,6 +105,7 @@ def launch_setup(context, *args, **kwargs):
     # gazebo_ros/launch/gazebo.launch.py
     #xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world'])
     xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('draw_svg'), 'worlds', 'draw_svg_world.sdf'])
+    #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={
@@ -143,6 +144,8 @@ def launch_setup(context, *args, **kwargs):
     load_controllers = []
     if load_controller.perform(context) in ('True', 'true'):
         for controller in controllers:
+            #print("Attempting to load: ", controller)
+            #input()
             load_controllers.append(Node(
                 package='controller_manager',
                 #executable='spawner.py',

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -1,47 +1,63 @@
-import os
+#!/usr/bin/env -S ros2 launch
-from launch import LaunchDescription
+"""Launch Python example for following a target"""
-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
 
+import os
 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 RegisterEventHandler, EmitEvent
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
+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.events import Shutdown
+from launch.actions import OpaqueFunction
 
 
 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='ufactory')
+    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
-    limited = LaunchConfiguration('limited', default=True)
+    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)
-    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
+    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='box')
-    geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
+    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.1)
+    geometry_radius = LaunchConfiguration('geometry_radius', default=0.005)
-    geometry_length = LaunchConfiguration('geometry_length', default=0.1)
+    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)
 
-    # robot moveit fake launch
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
-    # xarm_moveit_config/launch/_robot_moveit_fake.launch.py
+
-    robot_moveit_gazebo_fake_launch = IncludeLaunchDescription(
+    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
-        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,
@@ -49,11 +65,15 @@ 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': '6',
+            'dof': dof,
-            'robot_type': 'lite',
+            'robot_type': robot_type,
             'no_gui_ctrl': 'false',
-            'add_realsense_d435i': add_realsense_d435i,
+            '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_other_geometry': add_other_geometry,
             'geometry_type': geometry_type,
             'geometry_mass': geometry_mass,
@@ -66,12 +86,167 @@ 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('custom_xarm_gazebo'), 'launch', 'lite6_virtual_surface.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="virtual_drawing_surface",
+            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 = 'lite6'
+    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
-    ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware'
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
@@ -88,8 +263,8 @@ 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,
             'ros2_control_plugin': ros2_control_plugin,
             'add_realsense_d435i': add_realsense_d435i,
             'add_other_geometry': add_other_geometry,
@@ -107,8 +282,8 @@ def launch_setup(context, *args, **kwargs):
         },
         srdf_arguments={
             'prefix': prefix,
-            'dof': '6',
+            'dof': dof,
-            'robot_type': 'lite',
+            'robot_type': robot_type,
             'add_gripper': add_gripper,
             'add_vacuum_gripper': add_vacuum_gripper,
             'add_other_geometry': add_other_geometry,
@@ -119,32 +294,209 @@ def launch_setup(context, *args, **kwargs):
         }
     )
 
-    lite6_controller = Node(
+    load_yaml = getattr(mod, 'load_yaml')
-        package="lite6_controller",
+    controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
-        executable="lite6_controller",
+    ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
-        output="log",
+    kinematics_yaml = robot_description_parameters['robot_description_kinematics']
-        #arguments=["--ros-args", "--log-level", log_level],
+    joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
+
+    # 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,
-            #robot_description,
+            #ompl_planning_pipeline_config,
-            #robot_description_semantic,
+            pilz_planning_pipeline_config,
-            #{"use_sim_time": use_sim_time},
+            move_group_capabilities,
+            trajectory_execution,
+            plan_execution,
+            moveit_controllers,
+            planning_scene_monitor_parameters,
+            {'use_sim_time': use_sim_time},
         ],
     )
 
-    virtual_drawing_surface = Node(
+    # rviz with moveit configuration
-        package="virtual_drawing_surface",
+    # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
-        executable="drawing_surface.py",
+    rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
-        output="log",
+    rviz2_node = Node(
-        #arguments=["--ros-args", "--log-level", log_level],
+        package='rviz2',
-        #parameters=[{"use_sim_time": use_sim_time}],
+        executable='rviz2',
+        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 [
-        robot_moveit_gazebo_fake_launch,
+        #RegisterEventHandler(event_handler=OnProcessExit(
-        virtual_drawing_surface,
+        #    target_action=rviz2_node,
-        lite6_controller,
+        #    on_exit=[EmitEvent(event=Shutdown())]
-    ]
+        #)),
+        #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():

src/virtual_drawing_surface/src/py/drawing_surface.py

@@ -20,12 +20,21 @@ from PIL import ImageTk
 from PIL import Image as PImage
 import numpy as np
 
+def bound(val, lim1, lim2):
+    minval = min(lim1,lim2)
+    maxval = max(lim1,lim2)
+    val = max(minval, val)
+    val = min(maxval, val)
+    return val
 
 def translate(val, lmin, lmax, rmin, rmax):
+    #val = bound(val, lmin, lmax)
     lspan = lmax - lmin
     rspan = rmax - rmin
     val = float(val - lmin) / float(lspan)
-    return rmin + (val * rspan)
+    val = rmin + (val * rspan)
+    val = bound(val,rmin, rmax)
+    return val
 
 class DrawingApp(tk.Tk):
     def __init__(self, point_queue, image_queue):
@@ -33,12 +42,17 @@ class DrawingApp(tk.Tk):
 
         self.point_queue = point_queue
         self.image_queue = image_queue
-        self.width = 400
+
-        self.height = 400
+        #300dpi A4 paper is 2480x3508 px
+        #self.width = 3508
+        #self.height = 2480
+        #200dpi A4 paper is 1654x2339 px -->
+        self.width = 1654
+        self.height = 2339
         self.img = PImage.new("RGB", (self.width, self.height), (255, 255, 255))
         self.arr = np.array(self.img)
         self.frame_delay = 1 #ms
-        self.window_update_delay = 300 #ms
+        self.window_update_delay = 500 #ms
         self.counter = 0
         self.read_queue()
 
@@ -59,14 +73,12 @@ class DrawingApp(tk.Tk):
     def draw(self,x,y,z):
         # putpixel is too slow
         #self.img.putpixel((int(x), int(y)), (255, 0, 0))
-        self.arr[x,y,1] = 0
+        r = 4 # radius
-        self.arr[x,y,2] = 0
+        for xp in range(max(0, x-r), min(self.width-1, x+r)):
-        self.arr[x+1,y,1] = 0
+            for yp in range(max(0, y-r), min(self.height-1, y+r)):
-        self.arr[x+1,y,2] = 0
+                self.arr[xp,yp,0] = 0 #red
-        self.arr[x+1,y+1,1] = 0
+                self.arr[xp,yp,1] = 0 #green
-        self.arr[x+1,y+1,2] = 0
+                self.arr[xp,yp,2] = 0 #blue
-        self.arr[x,y+1,1] = 0
-        self.arr[x,y+1,2] = 0
         #print("Set x:{} y:{} to:{}".format(x,y,255))
 
     def draw_window(self):
@@ -87,9 +99,18 @@ class DrawingApp(tk.Tk):
             p = self.point_queue.get()
             #x = translate(p.x, -1.0, 0.5, 0, self.width)
             #y = translate(p.y, 0.5, -1.0, 0, self.height)
-            x = translate(p.x, -1.0, 0.5, 0, self.width)
+            #<pose>-0.1485 -0.3 0.5 0 0 0</pose>
-            y = translate(p.y, -1.0, 0.5, 0, self.height)
+            #<size>0.297 0.21</size>
-            self.draw(int(x),int(y),0)
+            #x = translate(p.x, -0.1485, 0.1485, 0, self.width)
+            #y = translate(p.y, -0.51, -0.3, 0, self.height)
+
+            x = int(translate(p.y, -0.5, 0.5, 0, self.width))
+            y = int(translate(p.x, -0.3485, 0.1, 0, self.height))
+
+            #x = bound(self.width - x, 0, self.width)
+            #y = bound(self.height - y, 0, self.height)
+
+            self.draw(x, y, 0)
 
         if self.counter >= self.window_update_delay:
             #print("Redraw")