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/README.md

@@ -0,0 +1 @@
+Taken from https://github.com/xArm-Developer/xarm_ros2/tree/humble 2023.03.21

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/README.md

@@ -0,0 +1 @@
+Taken from https://github.com/xArm-Developer/xarm_ros2/tree/humble 2023.03.21

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_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/README.md

@@ -0,0 +1 @@
+Taken from https://github.com/xArm-Developer/xarm_ros2/tree/humble 2023.03.21

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,221 @@
+#!/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
+
+from launch.actions import TimerAction
+
+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',
+            #'load_controller': 'false',
+        }.items(),
+    )
+
+    return [
+        robot_description_launch,
+        robot_moveit_common_launch,
+        joint_state_publisher_node,
+        ros2_control_launch,
+        # Wait before launching gazebo
+        TimerAction(period=5.0, actions=[
+            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/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

@@ -94,7 +94,8 @@ def launch_setup(context, *args, **kwargs):
     # 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'])),
+        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,
@@ -228,7 +229,7 @@ def launch_setup(context, *args, **kwargs):
             ],
         ),
         Node(
-            package="draw_svg",
+            package="virtual_drawing_surface",
             executable="drawing_surface.py",
             output="log",
             arguments=["--ros-args", "--log-level", log_level],

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")