Improve virtual surface

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

@@ -402,7 +402,7 @@
       <!--       topic name is always /odom -->
       <!-- <topic_name>pen_position</topic_name> -->
       <!--       Update rate in Hz -->
-      <update_rate>10</update_rate>
+      <update_rate>1000</update_rate>
       <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
       <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
     </plugin>

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/lite6_controller/launch/lite6_gazebo.launch.py

@@ -94,8 +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('custom_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'])),
+        #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -229,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")