Add script

Dockerfile

@@ -12,6 +12,8 @@ ENV WS_INSTALL_DIR=${WS_DIR}/install
 ENV WS_LOG_DIR=${WS_DIR}/log
 WORKDIR ${WS_DIR}
 
+COPY config.yaml ${WS_DIR}/
+
 ### Install Gazebo
 ARG IGNITION_VERSION=fortress
 ENV IGNITION_VERSION=${IGNITION_VERSION}
@@ -25,6 +27,9 @@ RUN apt-get update && \
     apt-get install -yq python3-pil.imagetk && \
     apt-get install -yq ros-${ROS_DISTRO}-pilz-industrial-motion-planner && \
     apt-get install -yq tmux && \
+    apt-get install -yq nano && \
+    apt-get install -yq vim && \
+    apt-get install -yq less && \
     apt-get install -yq python3-pip && \
     apt-get install -yq ros-${ROS_DISTRO}-desktop && \
     apt-get install -yq ros-${ROS_DISTRO}-rclcpp-components
@@ -49,7 +54,6 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     rm -rf ${WS_LOG_DIR}
 
 # Build packages
-COPY ./src/draw_svg ${WS_SRC_DIR}/draw_svg
 COPY ./src/drawing_controller ${WS_SRC_DIR}/drawing_controller
 COPY ./src/axidraw_controller ${WS_SRC_DIR}/axidraw_controller
 COPY ./src/virtual_drawing_surface ${WS_SRC_DIR}/virtual_drawing_surface
@@ -59,7 +63,7 @@ RUN pip install -r ${WS_SRC_DIR}/virtual_drawing_surface/requirements.txt
 RUN apt-get update
 RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     source "${WS_INSTALL_DIR}/local_setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/draw_svg ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller ${WS_SRC_DIR}/virtual_drawing_surface && \
+    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller ${WS_SRC_DIR}/virtual_drawing_surface && \
     rm -rf ${WS_LOG_DIR}
 
 # Build lite6 and xarm packages
@@ -76,7 +80,7 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     rm -rf ${WS_LOG_DIR}
 
 # Copy example svg images
-COPY ./svg svg
+COPY ./svg test-images
 
 ### Add workspace to the ROS entrypoint
 ### Source ROS workspace inside `~/.bashrc` to enable autocompletion

README.md

@@ -35,6 +35,23 @@ bash .docker/devel.bash
 ```
 This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
 
+Optionally you can pass a directory to the container with
+``` sh
+bash .docker/run.bash -v PATH_TO_SVG:/svg:rw
+```
+This will mount the given path to /svg in read-write mode in the container.
+
+
+#### Podman issues
+If using podman instead of docker, using the following will allow the container to access `/dev/` which is needed by the axidraw robot. 
+``` sh
+sudo bash .docker/build.bash
+```
+``` sh
+sudo bash .docker/devel.bash
+```
+Adding sudo may cause gazebo not to work, so it is recommended to use docker instead of podman.
+
 ## TODO Building locally
 
 Requirements:
@@ -60,26 +77,42 @@ DummyController echoes Motion messages to the terminal.
 ros2 run robot_controller dummy_controller
 ```
 
-AxidrawController draws on the axidraw robot
+### AxidrawController
+AxidrawController draws on the axidraw robot.
+Find the serial device in "/dev/", it is usually named "/dev/ttyACMX" where X is usually 0.
+Try a different serial port if the axidraw_controller continuously logs a message about failing to connect.
 ``` sh
-ros2 launch axidraw_controller axidraw_controller
+ros2 launch axidraw_controller axidraw_controller.launch.py serial_port:=/dev/ttyACM0 config:=./config.yaml
 ```
 
+### Lite6Controller
 This starts the simulated lite6
 ``` sh
-ros2 launch lite6_controller lite6_gazebo.launch.py
+ros2 launch lite6_controller lite6_gazebo.launch.py config:=./config.yaml
 ```
 
 This runs the real lite6
 ``` sh
-ros2 launch lite6_controller lite6_real.launch.py
+ros2 launch lite6_controller lite6_real.launch.py config:=./config.yaml
 ```
 
 This runs the real lite6 without Rviz (can be run on headless device over ssh)
 ``` sh
-ros2 launch lite6_controller lite6_real_no_gui.launch.py
+ros2 launch lite6_controller lite6_real_no_gui.launch.py config:=./config.yaml
 ```
 
+Before using the real lite6, it is recommended to run the calibration program.
+A lite6_controller must be running for calibration.
+This can be used to measure the Z height for a specific pen.
+The program also moves the arm to a known default position.
+``` sh
+ros2 run lite6_controller lite6_calibration
+```
+Follow the instructions, pressing enter when prompted.
+
+Change the Z-offset value accordingly.
+Restart the running lite6_controller after calibration.
+
 ### DrawingController
 Once a RobotController is running, simultaneously (using tmux or another terminal) run
 ``` sh
@@ -87,6 +120,23 @@ ros2 run drawing_controller drawing_controller svg/test.svg
 ```
 This will draw the svg image given as the last argument.
 
+### tmux workflow
+lite6 interface: http://192.168.1.150:18333
+
+#### Raspberry pi
+On the raspberry pi run 
+``` sh
+./setup_ros.sh
+```
+This will open a tmux session with the necessary ros2 packages sourced.
+#### Docker container
+``` sh
+tmux
+```
+
+If actively
+
+
 ## SVG compatibility info
 Tested with SVG from the following programs
 - Inkscape
@@ -177,7 +227,7 @@ And the following SVG path commands are supported:
 | MoveTo                 |  M, m             |             |
 | LineTo                 |  L, l, H, h, V, v |             |
 | Cubic Bézier Curve     |  C, c, S, s       |             |
-| Quadratic Bézier Curve |                   | Q, q, T, t  |
+| Quadratic Bézier Curve |  Q, q             | T, t        |
 | Elliptical Arc Curve   |                   | A, a        |
 | ClosePath              |  Z, z             |             |
 

config.yaml

@@ -0,0 +1,26 @@
+/robot_controller:
+  ros__parameters:
+    lite6_x_limit_lower: 0.1
+    lite6_x_limit_upper: 0.305
+    lite6_y_limit_lower: -0.1475
+    lite6_y_limit_upper: 0.1475
+
+    lite6_z_lift_amount: 0.01
+    lite6_z_offset: 0.201942
+
+    lite6_blend_radius: 0.0
+    lite6_max_velocity_scaling_factor: 1.0
+    lite6_max_acceleration_scaling_factor: 0.9
+
+/axidraw_serial:
+  ros__parameters:
+    axidraw_accel: 100
+    axidraw_speed_pendown: 50
+    axidraw_speed_penup: 50
+    axidraw_const_speed: false
+    axidraw_pen_delay_down: 0
+    axidraw_pen_delay_up: 0
+    axidraw_pen_pos_down: 40
+    axidraw_pen_pos_up: 60
+    axidraw_pen_rate_lower: 50
+    axidraw_pen_rate_raise: 75

scripts/log_drawing.sh

@@ -0,0 +1,12 @@
+#!/usr/bin/env sh
+# Logs drawn images
+# ./log_drawing.sh ROBOTNAME SVG
+# ROBOTNAME: robot name (logged before output)
+# SVG: svg path to be drawn
+
+date >> data.txt
+echo $1 >> data.txt
+ros2 run drawing_controller drawing_controller $2 2>&1 | grep 'Results' | tee -a data.txt
+echo '\n' >> data.txt
+
+#{'svg processing time': '00:00:02', 'failure': 65, 'total time': '00:00:09', 'drawing time': '00:00:06'}

src/axidraw_controller/CMakeLists.txt

@@ -40,6 +40,11 @@ install(PROGRAMS
     DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

src/axidraw_controller/config/config.yaml

@@ -0,0 +1,12 @@
+/axidraw_serial:
+  ros__parameters:
+    axidraw_accel: 100
+    axidraw_const_speed: false
+    axidraw_pen_delay_down: 0
+    axidraw_pen_delay_up: 0
+    axidraw_pen_pos_down: 40
+    axidraw_pen_pos_up: 60
+    axidraw_pen_rate_lower: 50
+    axidraw_pen_rate_raise: 75
+    axidraw_speed_pendown: 50
+    axidraw_speed_penup: 50

src/axidraw_controller/launch/axidraw_controller.launch.py

@@ -1,4 +1,5 @@
 import os
+import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -18,6 +19,13 @@ def launch_setup(context, *args, **kwargs):
     serial_port = LaunchConfiguration('serial_port', default='/dev/ttyACM0')
     log_level = LaunchConfiguration("log_level", default='info')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('axidraw_controller'), 'config', 'config.yaml']))
+
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        axidraw_config = yaml.safe_load(f)['/axidraw_serial']['ros__parameters']
+        print(f'Loaded configuration: {axidraw_config}')
+
     nodes = [
         Node(
             package="axidraw_controller",
@@ -34,6 +42,7 @@ def launch_setup(context, *args, **kwargs):
             arguments=["--ros-args", "--log-level", log_level],
             parameters=[
                 {"serial_port": serial_port},
+                axidraw_config,
             ],
         ),
     ]

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -162,7 +162,7 @@ class AxidrawController : public RobotController
         this->penup_pub->publish(std_msgs::msg::Empty());
       else
       {
-        this->pendown_pub->publish(std_msgs::msg::Empty());
+        //this->pendown_pub->publish(std_msgs::msg::Empty());
         while (i + count + 1 < points.size() && points[i + count + 1].z <= 0)
         {
           count++;

src/axidraw_controller/src/py/axidraw_serial.py

@@ -42,6 +42,7 @@ class AxidrawSerial(Node):
     status = {
         "serial": "not ready",
         "motion": "waiting",
+        "pen": "up",
     }
 
     def init_serial(self, port):
@@ -60,9 +61,22 @@ class AxidrawSerial(Node):
             return False
         self.ad.options.units = 2            # set working units to mm.
         self.ad.options.model = 2            # set model to AxiDraw V3/A3
+
+        self.ad.options.speed_pendown = self.get_parameter('axidraw_speed_pendown').get_parameter_value().integer_value # Maximum XY speed when the pen is down (plotting).
+        self.ad.options.speed_penup = self.get_parameter('axidraw_speed_penup').get_parameter_value().integer_value # Maximum XY speed when the pen is up.
+        self.ad.options.accel = self.get_parameter('axidraw_accel').get_parameter_value().integer_value  # Relative acceleration/deceleration speed.
+        #self.get_logger().error('accel:{}'.format(self.get_parameter('axidraw_accel').get_parameter_value().integer_value))
+        self.ad.options.pen_pos_down = self.get_parameter('axidraw_pen_pos_down').get_parameter_value().integer_value #Pen height when the pen is down (plotting).
+        self.ad.options.pen_pos_up = self.get_parameter('axidraw_pen_pos_up').get_parameter_value().integer_value #Pen height when the pen is up.
+        self.ad.options.pen_rate_lower = self.get_parameter('axidraw_pen_rate_lower').get_parameter_value().integer_value #  Speed of lowering the pen-lift motor.
+        self.ad.options.pen_rate_raise = self.get_parameter('axidraw_pen_rate_raise').get_parameter_value().integer_value #Speed of raising the pen-lift motor.
+        self.ad.options.const_speed = self.get_parameter('axidraw_const_speed').get_parameter_value().bool_value #Option: Use constant speed when pen is down.
+        self.ad.options.pen_delay_down = self.get_parameter('axidraw_pen_delay_down').get_parameter_value().integer_value #Added delay after lowering pen.
+        self.ad.options.pen_delay_up = self.get_parameter('axidraw_pen_delay_up').get_parameter_value().integer_value #Added delay after raising pen.
         self.ad.update()                     # Process changes to options
         self.status["serial"] = "ready"
         self.status["motion"] = "ready"
+        self.status["pen"] = "up"
         return True
 
     def __init__(self):
@@ -80,10 +94,22 @@ class AxidrawSerial(Node):
         self.declare_parameter('serial_port', '/dev/ttyACM0')
         port = self.get_parameter('serial_port').get_parameter_value().string_value
 
+        self.declare_parameter('axidraw_speed_pendown', 100)
+        self.declare_parameter('axidraw_speed_penup', 100)
+        self.declare_parameter('axidraw_accel', 100)
+        self.declare_parameter('axidraw_pen_pos_down', 40)
+        self.declare_parameter('axidraw_pen_pos_up', 60)
+        self.declare_parameter('axidraw_pen_rate_lower', 50)
+        self.declare_parameter('axidraw_pen_rate_raise', 75)
+        self.declare_parameter('axidraw_const_speed', False)
+        self.declare_parameter('axidraw_pen_delay_down', 0)
+        self.declare_parameter('axidraw_pen_delay_up', 0)
+
         self.status_srv = self.create_service(Status, 'axidraw_status', self.get_status)
 
         while not self.init_serial(port):
-            self.get_logger().error("Failed to connect to axidraw on port:{}".format(port))
+            self.get_logger().error("Failed to connect to axidraw on port:{}, retrying".format(port))
+        self.get_logger().info("Successfully connected to axidraw on port:{}".format(port))
 
         self.move_sub = self.create_subscription(Point, 'axidraw_move', self.move_callback, qos_profile=QoSProfile(depth=1))
         self.penup_sub = self.create_subscription(Empty, 'axidraw_penup', self.penup_callback, qos_profile=QoSProfile(depth=1))
@@ -165,7 +191,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received penup: {}".format(msg))
 
+        if self.status['pen'] == "down":
             self.ad.penup()
+            self.status['pen'] = "up"
         self.set_ready()
 
     def pendown_callback(self, msg):
@@ -178,7 +206,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received pendown: {}".format(msg))
 
+        if self.status['pen'] == "up":
             self.ad.pendown()
+            self.status['pen'] = "down"
         self.set_ready()
 
     def stroke_callback(self, msg):
@@ -193,6 +223,7 @@ class AxidrawSerial(Node):
 
         path = [ [p.x,p.y] for p in msg.points ]
         self.ad.draw_path(path)
+        self.status['pen'] = "up"
         self.set_ready()
 
 

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>

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

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

@@ -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,
@@ -104,7 +104,9 @@ 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('draw_svg'), 'worlds', 'draw_svg_world.sdf'])
+    #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={
@@ -143,6 +145,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/custom_xarm_gazebo/worlds/virtual_surface_world.sdf

@@ -1,10 +1,11 @@
 <?xml version="1.0"?>
 <!-- <sdf version="1.9"> -->
+<!-- <sdf version="1.7"> -->
 <sdf version="1.7">
-  <world name="draw_svg_world">
+  <world name="virtual_surface_world">
 
     <!-- Physics -->
-    <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
+    <!-- <plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
          <engine>
          <filename>ignition-physics-dartsim-plugin</filename>
          </engine>
@@ -16,19 +17,27 @@
          <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 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 filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
          </plugin>
-
+    -->
 
     <!--              -->
     <!-- Illumination -->
@@ -78,14 +87,14 @@
          </link>
     </model>
     <model name="drawing_surface">
-      <pose>-0.14 -0.3 0.5 0 0 0</pose>
+      <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.28 0.21</size>
+              <size>0.297 0.21</size>
             </plane>
           </geometry>
         </collision>
@@ -94,7 +103,7 @@
           <geometry>
              <plane>
                <normal>0 0 1</normal>
-               <size>0.28 0.21</size>
+               <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 -->
@@ -104,8 +113,12 @@
             <!-- <ros>
                  <remapping>~/image_raw:=/camera1/image_raw</remapping>
                  </ros> -->
-            <height>400</height>
+            <!-- 300dpi A4 paper is 2480x3508 px -->
-            <width>400</width>
+            <!-- <height>2480</height>
+                 <width>3508</width> -->
+            <!-- 200dpi A4 paper is 1654x2339 px -->
+            <height>1654</height>
+            <width>2339</width>
           </plugin>
         </visual>
         <!-- <inertial>
@@ -165,5 +178,10 @@
          <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/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 0.4
+  max_trans_acc: 4.00
+  max_trans_dec: -5.0
+  max_rot_vel: 1.57

src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml

@@ -4,31 +4,31 @@
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 30.0
   joint2:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 30.0
   joint3:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 30.0
   joint4:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 30.0
   joint5:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 30.0
   joint6:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
-    has_acceleration_limits: false
+    has_acceleration_limits: true
-    max_acceleration: 0.0
+    max_acceleration: 30.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/lib/robot_moveit_config_lib.py

@@ -65,19 +65,22 @@ def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None,
 
 
 def 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={},
     srdf_arguments={},
     arguments={}):
     urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware')
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     xarm_type = arguments.get('xarm_type', None)
     
     # 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')
 
+    joint_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml')
+    cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
+    joint_limits['cartesian_limits'] = cartesian_limits['cartesian_limits']
     return {
         'robot_description': get_xacro_file_content(
             xacro_file=xacro_urdf_file, 
@@ -87,6 +90,6 @@ def get_xarm_robot_description_parameters(
             xacro_file=xacro_srdf_file,
             arguments=srdf_arguments
         ),
-        'robot_description_planning': load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml'),
+        'robot_description_planning': joint_limits,
         'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
     }

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,7 +8,7 @@
   <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)" />
 

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>

src/draw_svg/CMakeLists.txt

@@ -1,59 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(draw_svg)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-#find_package(ign_moveit2_examples REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
-#find_package(moveit_visual_tools REQUIRED)
-
-# Install C++
-set(SRC_CPP_DIR src/cpp)
-# Example 0 - Follow target
-set(EXECUTABLE_0 follow)
-add_executable(${EXECUTABLE_0} ${SRC_CPP_DIR}/${EXECUTABLE_0}.cpp)
-ament_target_dependencies(${EXECUTABLE_0}
-    rclcpp
-    geometry_msgs
-    moveit_ros_planning_interface
-)
-install(TARGETS
-    ${EXECUTABLE_0}
-    DESTINATION lib/${PROJECT_NAME}
-)
-
-# Install Python
-set(SRC_PY_DIR src/py)
-install(PROGRAMS
-    ${SRC_PY_DIR}/draw_svg.py
-    ${SRC_PY_DIR}/follow.py
-    ${SRC_PY_DIR}/drawing_surface.py
-    DESTINATION lib/${PROJECT_NAME}
-)
-install(PROGRAMS
-    ${SRC_PY_DIR}/robots/lite6.py
-    DESTINATION lib/${PROJECT_NAME}/robots/
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-# Install directories
-install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME})
-
-ament_package()

src/draw_svg/README.md

@@ -1,4 +0,0 @@
-# draw_svg
-WILL BE REMOVED
-
-Contains initial testing of libraries and launch files.

src/draw_svg/launch/default.launch.py

@@ -1,146 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch default world with the default robot (configurable)"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    world_type = LaunchConfiguration("world_type")
-    robot_type = LaunchConfiguration("robot_type")
-    rviz_config = LaunchConfiguration("rviz_config")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # Determine what world/robot combination to launch
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "__world_launch_basename",
-            default_value=["world_", world_type, ".launch.py"],
-        ),
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "__robot_launch_basename",
-            default_value=["robot_", robot_type, ".launch.py"],
-        ),
-    )
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch Ignition Gazebo with the required ROS<->IGN bridges
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("draw_svg"),
-                        "launch",
-                        "worlds",
-                        LaunchConfiguration("__world_launch_basename"),
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-        # Spawn robot
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("draw_svg"),
-                        "launch",
-                        "robots",
-                        LaunchConfiguration("__robot_launch_basename"),
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-        # Launch move_group of MoveIt 2
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [FindPackageShare("xarm_moveit_config"), "launch", "lite6_moveit_fake.launch.py"]
-                )
-            ),
-            launch_arguments=[
-                ("ros2_control_plugin", "ign"),
-                ("ros2_control_command_interface", "effort"),
-                # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled
-                ("collision_arm", "false"),
-                ("rviz_config", rviz_config),
-                ("use_sim_time", use_sim_time),
-                ("log_level", log_level),
-            ],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # World selection
-        DeclareLaunchArgument(
-            "world_type",
-            default_value="default",
-            description="Name of the world configuration to load.",
-        ),
-        # Robot selection
-        DeclareLaunchArgument(
-            "robot_type",
-            default_value="panda",
-            description="Name of the robot type to use.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "rviz_config",
-            default_value=path.join(
-                get_package_share_directory("draw_svg"),
-                "rviz",
-                "ign_moveit2_examples.rviz",
-            ),
-            description="Path to configuration for RViz2.",
-        ),
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

src/draw_svg/launch/draw_svg.launch.py

@@ -1,112 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch Python example for following a target"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    robot_type = LaunchConfiguration("robot_type")
-    rviz_config = LaunchConfiguration("rviz_config")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch world with robot (configured for this example)
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("draw_svg"),
-                        "launch",
-                        "default.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[
-                ("world_type", "draw_svg"),
-                ("robot_type", robot_type),
-                ("rviz_config", rviz_config),
-                ("use_sim_time", use_sim_time),
-                ("ign_verbosity", ign_verbosity),
-                ("log_level", log_level),
-            ],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # Run the example node (Python)
-        Node(
-            package="draw_svg",
-            executable="follow.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-        Node(
-            package="draw_svg",
-            executable="draw_svg.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # Robot selection
-        DeclareLaunchArgument(
-            "robot_type",
-            default_value="lite6",
-            description="Name of the robot to use.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "rviz_config",
-            default_value=path.join(
-                get_package_share_directory("xarm_description"),
-                "rviz",
-                "display.rviz",
-            ),
-            description="Path to configuration for RViz2.",
-        ),
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

src/draw_svg/launch/lite6_pen.launch.py

@@ -1,255 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch Python example for following a target"""
-
-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, Command, FindExecutable
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch.event_handlers import OnProcessExit
-from launch.actions import OpaqueFunction
-
-
-def launch_setup(context, *args, **kwargs):
-    use_sim_time = LaunchConfiguration("use_sim_time", default=True)
-    log_level = LaunchConfiguration("log_level", default='warn')
-    rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz"))
-
-    prefix = LaunchConfiguration('prefix', default='')
-    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
-    limited = LaunchConfiguration('limited', default=False)
-    effort_control = LaunchConfiguration('effort_control', default=False)
-    velocity_control = LaunchConfiguration('velocity_control', default=False)
-    add_gripper = LaunchConfiguration('add_gripper', default=False)
-    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
-    dof = LaunchConfiguration('dof', default=6)
-    robot_type = LaunchConfiguration('robot_type', default='lite')
-    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
-
-    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
-    #geometry_type = LaunchConfiguration('geometry_type', default='box')
-    geometry_type = LaunchConfiguration('geometry_type', default='cylinder')
-    geometry_mass = LaunchConfiguration('geometry_mass', default=0.05)
-    geometry_height = LaunchConfiguration('geometry_height', default=0.1)
-    geometry_radius = LaunchConfiguration('geometry_radius', default=0.005)
-    geometry_length = LaunchConfiguration('geometry_length', default=0.07)
-    geometry_width = LaunchConfiguration('geometry_width', default=0.1)
-    geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
-    geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
-    geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
-    geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
-    geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
-    load_controller = LaunchConfiguration('load_controller', default=True)
-
-    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
-
-
-    ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem'
-    controllers_name = 'fake_controllers'
-    moveit_controller_manager_key = 'moveit_simple_controller_manager'
-    moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
-
-    # robot 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'])),
-        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': 'false',
-            '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,
-            '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,
-            'use_sim_time': 'true'
-        }.items(),
-    )
-
-    # robot gazebo launch
-    # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
-    robot_gazebo_launch = IncludeLaunchDescription(
-        #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
-        launch_arguments={
-            'prefix': prefix,
-            'hw_ns': hw_ns,
-            'limited': limited,
-            'effort_control': effort_control,
-            'velocity_control': velocity_control,
-            'add_gripper': add_gripper,
-            'add_vacuum_gripper': add_vacuum_gripper,
-            'dof': dof,
-            'robot_type': robot_type,
-            'ros2_control_plugin': ros2_control_plugin,
-            'load_controller': 'true',
-        }.items(),
-    )
-
-    # URDF
-    _robot_description_xml = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']
-            ),
-            #PathJoinSubstitution(
-            #    [
-            #        FindPackageShare('xarm_description'),
-            #        "urdf",
-            #        "lite6",
-            #        #"lite6.urdf.xacro",
-            #        "lite6_robot_macro.xacro",
-            #    ]
-            #),
-            " ",
-            #"name:=", "lite6", " ",
-            "prefix:=", " ",
-            "hw_ns:=", hw_ns, " ",
-            "limited:=", limited, " ",
-            "effort_control:=", effort_control, " ",
-            "velocity_control:=", velocity_control, " ",
-            "add_gripper:=", add_gripper, " ",
-            "add_vacuum_gripper:=", add_vacuum_gripper, " ",
-            "dof:=", dof, " ",
-            "robot_type:=", robot_type, " ",
-            "ros2_control_plugin:=", ros2_control_plugin, " ",
-            #"ros2_control_params:=", ros2_control_params, " ",
-
-            "add_other_geometry:=", add_other_geometry, " ",
-            "geometry_type:=", geometry_type, " ",
-            "geometry_mass:=", geometry_mass, " ",
-            "geometry_height:=", geometry_height, " ",
-            "geometry_radius:=", geometry_radius, " ",
-            "geometry_length:=", geometry_length, " ",
-            "geometry_width:=", geometry_width, " ",
-            "geometry_mesh_filename:=", geometry_mesh_filename, " ",
-            "geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
-            "geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
-            "geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
-            "geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
-
-            #"robot_ip:=", robot_ip, " ",
-            #"report_type:=", report_type, " ",
-            #"baud_checkset:=", baud_checkset, " ",
-            #"default_gripper_baud:=", default_gripper_baud, " ",
-        ]
-    )
-    # TODO fix URDF loading
-    # xacro urdf/xarm_pen.urdf.xacro prefix:= hw_ns:=xarm dof:=6 limited:=false effort_control:=false velocity_control:=false add_gripper:=false add_vacuum_gripper:=false robot_type:=lite ros2_control_plugin:=gazebo_ros2_control/GazeboSystem add_other_geometry:=false geometry_type:=cylinder geometry_mass:=0.05 geometry_height:=0.1 geometry_radius:=0.005 geometry_length:=0.07 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:="0 0 0" geometry_mesh_origin_rpy:="0 0 0" geometry_mesh_tcp_xyz:="0 0 0" geometry_mesh_tcp_rpy:="0 0 0"
-    _robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])])
-    robot_description = {"robot_description": _robot_description_xml}
-    # SRDF
-    _robot_description_semantic_xml = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare('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="draw_svg",
-            executable="follow",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[
-                robot_description,
-                robot_description_semantic,
-                {"use_sim_time": use_sim_time},
-            ],
-        ),
-        Node(
-            package="draw_svg",
-            executable="draw_svg.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-        Node(
-            package="draw_svg",
-            executable="drawing_surface.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return [
-        robot_moveit_common_launch,
-        robot_gazebo_launch,
-    ] + nodes
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        OpaqueFunction(function=launch_setup)
-    ])

src/draw_svg/launch/lite6_real.launch.py

@@ -1,361 +0,0 @@
-import os
-from launch import LaunchDescription
-from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
-from launch_ros.actions import Node
-
-from ament_index_python import get_package_share_directory
-from launch.launch_description_sources import load_python_launch_file_as_module
-from launch import LaunchDescription
-from launch.actions import RegisterEventHandler, EmitEvent
-from launch.event_handlers import OnProcessExit
-from launch.events import Shutdown
-
-
-def launch_setup(context, *args, **kwargs):
-    robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
-    report_type = LaunchConfiguration('report_type', default='normal')
-    prefix = LaunchConfiguration('prefix', default='')
-    hw_ns = LaunchConfiguration('hw_ns', default='ufactory')
-    limited = LaunchConfiguration('limited', default=True)
-    effort_control = LaunchConfiguration('effort_control', default=False)
-    velocity_control = LaunchConfiguration('velocity_control', default=False)
-    add_gripper = LaunchConfiguration('add_gripper', default=False)
-    add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
-    dof = LaunchConfiguration('dof', default=6)
-    robot_type = LaunchConfiguration('robot_type', default='lite')
-    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
-
-    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
-    geometry_type = LaunchConfiguration('geometry_type', default='box')
-    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"')
-
-    baud_checkset = LaunchConfiguration('baud_checkset', default=True)
-    default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000)
-
-    ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware'
-    controllers_name = LaunchConfiguration('controllers_name', default='controllers')
-    moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
-    moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_simple_controller_manager/MoveItSimpleControllerManager')
-
-    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
-    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
-
-    use_sim_time = LaunchConfiguration('use_sim_time', default=False)
-    log_level = LaunchConfiguration("log_level", default='warn')
-
-    # # robot driver launch
-    # # xarm_api/launch/_robot_driver.launch.py
-    # robot_driver_launch = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])),
-    #     launch_arguments={
-    #         'robot_ip': robot_ip,
-    #         'report_type': report_type,
-    #         'dof': dof,
-    #         'hw_ns': hw_ns,
-    #         'add_gripper': add_gripper,
-    #         'prefix': prefix,
-    #         'baud_checkset': baud_checkset,
-    #         'default_gripper_baud': default_gripper_baud,
-    #         'robot_type': robot_type,
-    #     }.items(),
-    # )
-
-    # robot description launch
-    # xarm_description/launch/_robot_description.launch.py
-    robot_description_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
-        launch_arguments={
-            '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': PathJoinSubstitution(['/', ros_namespace, hw_ns, 'joint_states']),
-            '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_description_parameters
-    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'xarm_moveit_config'
-    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_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
-        urdf_arguments={
-            'prefix': prefix,
-            'hw_ns': hw_ns.perform(context).strip('/'),
-            '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_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,
-        },
-        srdf_arguments={
-            'prefix': prefix,
-            'dof': dof,
-            'robot_type': robot_type,
-            'add_gripper': add_gripper,
-            'add_vacuum_gripper': add_vacuum_gripper,
-            'add_other_geometry': add_other_geometry,
-        },
-        arguments={
-            'context': context,
-            'xarm_type': xarm_type,
-        }
-    )
-
-    load_yaml = getattr(mod, 'load_yaml')
-    controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
-    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)
-
-    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))
-
-    # Planning Configuration
-    ompl_planning_pipeline_config = {
-        'move_group': {
-            'planning_plugin': 'ompl_interface/OMPLPlanner',
-            'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            'start_state_max_bounds_error': 0.1,
-        }
-    }
-    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
-
-    # Moveit controllers Configuration
-    moveit_controllers = {
-        moveit_controller_manager_key.perform(context): controllers_yaml,
-        'moveit_controller_manager': moveit_controller_manager_value.perform(context),
-    }
-
-    # Trajectory Execution Configuration
-    trajectory_execution = {
-        'moveit_manage_controllers': True,
-        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
-        'trajectory_execution.allowed_goal_duration_margin': 0.5,
-        'trajectory_execution.allowed_start_tolerance': 0.01,
-        'trajectory_execution.execution_duration_monitoring': False
-    }
-
-    plan_execution = {
-        'plan_execution.record_trajectory_state_frequency': 10.0,
-    }
-
-    planning_scene_monitor_parameters = {
-        'publish_planning_scene': True,
-        'publish_geometry_updates': True,
-        'publish_state_updates': True,
-        'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
-        #     "name": "planning_scene_monitor",
-        #     "robot_description": "robot_description",
-        #     "joint_state_topic": "/joint_states",
-        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
-        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
-        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
-        #     "wait_for_initial_state_timeout": 10.0,
-        # },
-    }
-
-    # Start the actual move_group node/action server
-    move_group_node = Node(
-        package='moveit_ros_move_group',
-        executable='move_group',
-        output='screen',
-        parameters=[
-            robot_description_parameters,
-            ompl_planning_pipeline_config,
-            trajectory_execution,
-            plan_execution,
-            moveit_controllers,
-            planning_scene_monitor_parameters,
-            {'use_sim_time': use_sim_time},
-        ],
-    )
-
-    # rviz with moveit configuration
-    # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
-    rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
-    rviz2_node = Node(
-        package='rviz2',
-        executable='rviz2',
-        name='rviz2',
-        output='screen',
-        arguments=['-d', rviz_config_file],
-        parameters=[
-            robot_description_parameters,
-            ompl_planning_pipeline_config,
-            {'use_sim_time': use_sim_time},
-        ],
-        remappings=[
-            ('/tf', 'tf'),
-            ('/tf_static', 'tf_static'),
-        ]
-    )
-
-    # Static TF
-    static_tf = Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
-        name='static_transform_publisher',
-        output='screen',
-        arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link_base'],
-    )
-
-
-    # joint state publisher node
-    joint_state_publisher_node = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher',
-        output='screen',
-        parameters=[{'source_list': ['{}/joint_states'.format(hw_ns.perform(context))]}],
-        remappings=[
-            ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)),
-        ],
-    )
-
-    # ros2 control launch
-    # xarm_controller/launch/_ros2_control.launch.py
-    ros2_control_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_ros2_control.launch.py'])),
-        launch_arguments={
-            'robot_ip': robot_ip,
-            'report_type': report_type,
-            'baud_checkset': baud_checkset,
-            'default_gripper_baud': default_gripper_baud,
-            'prefix': prefix,
-            'hw_ns': hw_ns,
-            'limited': limited,
-            'effort_control': effort_control,
-            'velocity_control': velocity_control,
-            'add_gripper': add_gripper,
-            'add_vacuum_gripper': add_vacuum_gripper,
-            'dof': dof,
-            'robot_type': robot_type,
-            'ros2_control_plugin': ros2_control_plugin,
-        }.items(),
-    )
-
-    control_node = Node(
-        package='controller_manager',
-        executable='spawner',
-        output='screen',
-        arguments=[
-            '{}{}_traj_controller'.format(prefix.perform(context), xarm_type),
-            '--controller-manager', '{}/controller_manager'.format(ros_namespace)
-        ],
-    )
-
-    nodes = [
-        Node(
-            package="draw_svg",
-            executable="follow",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[
-                #robot_description_parameters['robot_description'],
-                #robot_description_parameters['robot_description_semantic'],
-                #robot_description_parameters['robot_description_planning'],
-                #robot_description_parameters['robot_description_kinematics'],
-                robot_description_parameters,
-                {"use_sim_time": use_sim_time},
-            ],
-        ),
-        Node(
-            package="draw_svg",
-            executable="draw_svg.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return [
-
-        RegisterEventHandler(event_handler=OnProcessExit(
-            target_action=rviz2_node,
-            on_exit=[EmitEvent(event=Shutdown())]
-        )),
-        rviz2_node,
-        static_tf,
-        move_group_node,
-
-
-        robot_description_launch,
-        joint_state_publisher_node,
-        ros2_control_launch,
-        control_node,
-        # robot_driver_launch,
-    ] + nodes
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        OpaqueFunction(function=launch_setup)
-    ])

src/draw_svg/launch/log_position.launch.py

@@ -1,41 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch Python example for following a target"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-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 generate_launch_description() -> LaunchDescription:
-    log_position_node = Node(
-        package="draw_svg",
-        executable="log_position",
-        output='log',
-        arguments=[
-        ],
-    )
-    return LaunchDescription([log_position_node,])
-
-def generate_declared_arguments():
-    return [
-        OpaqueFunction(function=launch_setup)
-    ]

src/draw_svg/launch/robots/_robot_beside_table_gazebo.launch.py

@@ -1,168 +0,0 @@
-#!/usr/bin/env python3
-
-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('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']),
-            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', 'follow_target.sdf'])
-    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',
-            '-y', '-0.5',
-            '-z', '1.021',
-            '-Y', '1.571',
-        ],
-    )
-
-    # 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:
-            load_controllers.append(Node(
-                package='controller_manager',
-                executable='spawner.py',
-                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/draw_svg/launch/robots/robot_lite6.launch.py

@@ -1,61 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch script for spawning ufactory xarm lite6 into Ignition Gazebo world"""
-
-from typing import List
-
-from launch_ros.actions import Node
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    model = LaunchConfiguration("model")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_gazebo_create
-        Node(
-            package="ros_ign_gazebo",
-            executable="create",
-            output="log",
-            arguments=["-file", model, "--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # Model for Ignition Gazebo
-        DeclareLaunchArgument(
-            "model",
-            default_value="lite6",
-            description="Name or filepath of model to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

src/draw_svg/launch/robots/robot_panda.launch.py

@@ -1,61 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch script for spawning Franka Emika Panda into Ignition Gazebo world"""
-
-from typing import List
-
-from launch_ros.actions import Node
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    model = LaunchConfiguration("model")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_gazebo_create
-        Node(
-            package="ros_ign_gazebo",
-            executable="create",
-            output="log",
-            arguments=["-file", model, "--ros-args", "--log-level", log_level],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # Model for Ignition Gazebo
-        DeclareLaunchArgument(
-            "model",
-            default_value="lite6",
-            description="Name or filepath of model to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

src/draw_svg/launch/worlds/world_default.launch.py

@@ -1,91 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch default.sdf and the required ROS<->IGN bridges"""
-
-from typing import List
-
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    world = LaunchConfiguration("world")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch Ignition Gazebo
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ros_ign_gazebo"),
-                        "launch",
-                        "ign_gazebo.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_bridge (clock -> ROS 2)
-        Node(
-            package="ros_ign_bridge",
-            executable="parameter_bridge",
-            output="log",
-            arguments=[
-                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
-                "--ros-args",
-                "--log-level",
-                log_level,
-            ],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # World for Ignition Gazebo
-        DeclareLaunchArgument(
-            "world",
-            default_value="default.sdf",
-            description="Name or filepath of world to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

src/draw_svg/launch/worlds/world_draw_svg.launch.py

@@ -1,113 +0,0 @@
-#!/usr/bin/env -S ros2 launch
-"""Launch worlds/follow_target.sdf and the required ROS<->IGN bridges"""
-
-from os import path
-from typing import List
-
-from ament_index_python.packages import get_package_share_directory
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-
-
-def generate_launch_description() -> LaunchDescription:
-
-    # Declare all launch arguments
-    declared_arguments = generate_declared_arguments()
-
-    # Get substitution for all arguments
-    world = LaunchConfiguration("world")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    ign_verbosity = LaunchConfiguration("ign_verbosity")
-    log_level = LaunchConfiguration("log_level")
-
-    # List of included launch descriptions
-    launch_descriptions = [
-        # Launch Ignition Gazebo
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                PathJoinSubstitution(
-                    [
-                        FindPackageShare("ros_ign_gazebo"),
-                        "launch",
-                        "ign_gazebo.launch.py",
-                    ]
-                )
-            ),
-            launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])],
-        ),
-    ]
-
-    # List of nodes to be launched
-    nodes = [
-        # ros_ign_bridge (clock -> ROS 2)
-        Node(
-            package="ros_ign_bridge",
-            executable="parameter_bridge",
-            output="log",
-            arguments=[
-                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
-                "--ros-args",
-                "--log-level",
-                log_level,
-            ],
-            parameters=[{"use_sim_time": use_sim_time}],
-        ),
-        # ros_ign_bridge (target pose -> ROS 2)
-        #Node(
-        #    package="ros_ign_bridge",
-        #    executable="parameter_bridge",
-        #    output="log",
-        #    arguments=[
-        #        "/model/target/pose"
-        #        + "@"
-        #        + "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose",
-        #        "--ros-args",
-        #        "--log-level",
-        #        log_level,
-        #    ],
-        #    parameters=[{"use_sim_time": use_sim_time}],
-        #    remappings=[("/model/target/pose", "/target_pose")],
-        #),
-    ]
-
-    return LaunchDescription(declared_arguments + launch_descriptions + nodes)
-
-
-def generate_declared_arguments() -> List[DeclareLaunchArgument]:
-    """
-    Generate list of all launch arguments that are declared for this launch script.
-    """
-
-    return [
-        # World for Ignition Gazebo
-        DeclareLaunchArgument(
-            "world",
-            default_value=path.join(
-                get_package_share_directory("draw_svg"),
-                "worlds",
-                "follow_target.sdf",
-            ),
-            description="Name or filepath of world to load.",
-        ),
-        # Miscellaneous
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="true",
-            description="If true, use simulated clock.",
-        ),
-        DeclareLaunchArgument(
-            "ign_verbosity",
-            default_value="2",
-            description="Verbosity level for Ignition Gazebo (0~4).",
-        ),
-        DeclareLaunchArgument(
-            "log_level",
-            default_value="warn",
-            description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
-        ),
-    ]

src/draw_svg/launch/xarm_draw_svg.launch.py

@@ -1,135 +0,0 @@
-#!/usr/bin/env python3
-
-from launch import LaunchDescription
-from launch_ros.actions import Node
-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
-
-
-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=6)
-    robot_type = LaunchConfiguration('robot_type', default='lite')
-    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
-
-    add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
-    geometry_type = LaunchConfiguration('geometry_type', default='box')
-    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 = 'gazebo_ros2_control/GazeboSystem'
-    controllers_name = 'fake_controllers'
-    moveit_controller_manager_key = 'moveit_simple_controller_manager'
-    moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager'
-
-    # robot 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'])),
-        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_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,
-            'use_sim_time': 'true'
-        }.items(),
-    )
-
-    # robot gazebo launch
-    # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
-    robot_gazebo_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', '_robot_beside_table_gazebo.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(),
-    )
-
-    # List of nodes to be launched
-        # Run the example node (Python)
-    followNode = Node(
-            package="draw_svg",
-            executable="follow.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", "warn"],
-            parameters=[{"use_sim_time": True}],
-        )
-    drawNode = Node(
-            package="draw_svg",
-            executable="draw_svg.py",
-            output="log",
-            arguments=["--ros-args", "--log-level", "warn"],
-            parameters=[{"use_sim_time": True}],
-        )
-
-    # ros_ign_gazebo_create
-    #model = LaunchConfiguration("model")
-    #ros_ign_bridge = Node(
-    #        package="ros_ign_gazebo",
-    #        executable="create",
-    #        output="log",
-    #        arguments=["-file", model, "--ros-args", "--log-level", log_level],
-    #        parameters=[{"use_sim_time": use_sim_time}],
-    #    )
-
-    return [
-        robot_gazebo_launch,
-        robot_moveit_common_launch,
-        followNode,
-        drawNode,
-        #ros_ign_bridge,
-    ]
-
-
-def generate_launch_description():
-    return LaunchDescription([
-        OpaqueFunction(function=launch_setup)
-    ])

src/draw_svg/package.xml

@@ -1,32 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>draw_svg</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="root@todo.todo">root</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <!-- <depend>ign_moveit2_examples</depend> -->
-  <depend>rclcpp</depend>
-  <depend>geometry_msgs</depend>
-  <depend>tf2_ros</depend>
-  <depend>python3-lxml</depend>
-  <depend>python3-tk</depend>
-  <depend>python-imaging</depend>
-  <depend>python-numpy</depend>
-  <depend>python3-pil.imagetk</depend>
-  <depend>xarm_description</depend>
-  <depend>xarm_moveit_config</depend>
-  <!-- <depend>moveit_visual_tools</depend> -->
-  <!-- <depend>moveit_ros_planning_interface</depend> -->
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/draw_svg/rebuild.sh

@@ -1,4 +0,0 @@
-rm -r build/ install/ log/
-rosdep install --from-paths . --ignore-src -r -y
-colcon build
-#ros2 launch draw_svg draw_svg.launch.py

src/draw_svg/rviz/ign_moveit2_examples.rviz

@@ -1,337 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 0
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /MotionPlanning1
-      Splitter Ratio: 0.6278260946273804
-    Tree Height: 412
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-      - /Current View1/Focal Point1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz_common/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Acceleration_Scaling_Factor: 1
-      Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: true
-      Move Group Namespace: ""
-      MoveIt_Allow_Approximate_IK: false
-      MoveIt_Allow_External_Program: false
-      MoveIt_Allow_Replanning: false
-      MoveIt_Allow_Sensor_Positioning: false
-      MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 5
-      MoveIt_Use_Cartesian_Path: false
-      MoveIt_Use_Constraint_Aware_IK: false
-      MoveIt_Warehouse_Host: 127.0.0.1
-      MoveIt_Warehouse_Port: 33829
-      MoveIt_Workspace:
-        Center:
-          X: 0
-          Y: 0
-          Z: 0
-        Size:
-          X: 2
-          Y: 2
-          Z: 2
-      Name: MotionPlanning
-      Planned Path:
-        Color Enabled: false
-        Interrupt Display: false
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          panda_hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_hand_tcp:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          panda_leftfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link8:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          panda_rightfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-        Loop Animation: true
-        Robot Alpha: 0.25
-        Robot Color: 150; 50; 150
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 0.1s
-        Trail Step Size: 1
-        Trajectory Topic: /display_planned_path
-      Planning Metrics:
-        Payload: 1
-        Show Joint Torques: false
-        Show Manipulability: false
-        Show Manipulability Index: false
-        Show Weight Limit: false
-        TextHeight: 0.07999999821186066
-      Planning Request:
-        Colliding Link Color: 255; 0; 0
-        Goal State Alpha: 0.5
-        Goal State Color: 250; 128; 0
-        Interactive Marker Size: 0.10000000149011612
-        Joint Violation Color: 255; 0; 255
-        Planning Group: arm
-        Query Goal State: true
-        Query Start State: false
-        Show Workspace: false
-        Start State Alpha: 0.5
-        Start State Color: 0; 255; 0
-      Planning Scene Topic: monitored_planning_scene
-      Robot Description: robot_description
-      Scene Geometry:
-        Scene Alpha: 0.8999999761581421
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
-      Scene Robot:
-        Attached Body Color: 150; 50; 150
-        Links:
-          All Links Enabled: true
-          Expand Joint Details: false
-          Expand Link Details: false
-          Expand Tree: false
-          Link Tree Style: Links in Alphabetic Order
-          panda_hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_hand_tcp:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          panda_leftfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link0:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link1:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link2:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link3:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link4:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link5:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link6:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link7:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          panda_link8:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          panda_rightfinger:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-        Robot Alpha: 1
-        Show Robot Collision: false
-        Show Robot Visual: true
-      Value: true
-      Velocity_Scaling_Factor: 1
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: panda_link0
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 1.5
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0.3333333432674408
-        Y: 0
-        Z: 0.5
-      Focal Shape Fixed Size: false
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.39269909262657166
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.7853981852531433
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 720
-  Hide Left Dock: false
-  Hide Right Dock: true
-  MotionPlanning:
-    collapsed: false
-  MotionPlanning - Trajectory Slider:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000247000003b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000001eb000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000232000001c4000001b9010000030000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000046000003b0000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000048fc0100000002fb0000000800540069006d0065000000000000000780000002d701000003fb0000000800540069006d0065010000000000000450000000000000000000000538000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1280
-  X: 0
-  Y: 0

src/draw_svg/src/cpp/draw_svg.cpp

@@ -1,10 +0,0 @@
-#include <cstdio>
-
-int main(int argc, char ** argv)
-{
-  (void) argc;
-  (void) argv;
-
-  printf("hello world draw_svg package\n");
-  return 0;
-}

src/draw_svg/src/cpp/follow.cpp

@@ -1,141 +0,0 @@
-//#include "follow.h"
-
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/pose.hpp>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <rclcpp/qos.hpp>
-#include <rclcpp/rclcpp.hpp>
-
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit_msgs/msg/collision_object.hpp>
-//#include <moveit_visual_tools/moveit_visual_tools.h>
-#include <std_msgs/msg/color_rgba.hpp>
-
-const std::string MOVE_GROUP = "lite6";
-
-class MoveItFollowTarget : public rclcpp::Node
-{
-public:
-  /// Constructor
-  MoveItFollowTarget();
-
-  /// Move group interface for the robot
-  moveit::planning_interface::MoveGroupInterface move_group_;
-  /// Subscriber for target pose
-  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
-  /// Target pose that is used to detect changes
-  geometry_msgs::msg::Pose previous_target_pose_;
-
-  bool moved = false;
-  std::vector<geometry_msgs::msg::Pose> waypoints;
-
-
-private:
-  /// Callback that plans and executes trajectory each time the target pose is changed
-  void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
-};
-
-MoveItFollowTarget::MoveItFollowTarget() : Node("follow_target"),
-                                           move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
-{
-  // Use upper joint velocity and acceleration limits
-  this->move_group_.setMaxAccelerationScalingFactor(1.0);
-  this->move_group_.setMaxVelocityScalingFactor(1.0);
-
-  // Subscribe to target pose
-  target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
-
-  RCLCPP_INFO(this->get_logger(), "Initialization successful.");
-}
-
-void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
-{
-  // Return if target pose is unchanged
-  if (msg->pose == this->previous_target_pose_)
-  {
-    return;
-  }
-
-  RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing...");
-
-  //if (this->moved)
-  if (false)
-  {
-    //https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp
-    //
-    moveit_msgs::msg::PositionConstraint plane_constraint;
-    plane_constraint.header.frame_id = this->move_group_.getPoseReferenceFrame();
-    plane_constraint.link_name = this->move_group_.getEndEffectorLink();
-    shape_msgs::msg::SolidPrimitive plane;
-    plane.type = shape_msgs::msg::SolidPrimitive::BOX;
-    //plane.dimensions = { 5.0, 0.0005, 5.0 };
-    plane.dimensions = { 5.0, 1.0, 5.0 };
-    plane_constraint.constraint_region.primitives.emplace_back(plane);
-
-    geometry_msgs::msg::Pose plane_pose;
-    plane_pose.position.x = 0.14;
-    plane_pose.position.y = -0.3;
-    plane_pose.position.z = 1.0;
-    plane_pose.orientation.x = 0.0;
-    plane_pose.orientation.y = 0.0;
-    plane_pose.orientation.z = 0.0;
-    plane_pose.orientation.w = 0.0;
-    plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose);
-    plane_constraint.weight = 1.0;
-
-    moveit_msgs::msg::Constraints plane_constraints;
-    plane_constraints.position_constraints.emplace_back(plane_constraint);
-    plane_constraints.name = "use_equality_constraints";
-    this->move_group_.setPathConstraints(plane_constraints);
-
-    // And again, configure and solve the planning problem
-    this->move_group_.setPoseTarget(msg->pose);
-    this->move_group_.setPlanningTime(10.0);
-    //success = (this->move_group_.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
-    //RCLCPP_INFO(this->get_logger(), "Plan with plane constraint %s", success ? "" : "FAILED");
-    this->move_group_.move();
-  }
-  //else
-  //{
-  //  // Plan and execute motion
-  //  this->move_group_.setPoseTarget(msg->pose);
-  //  this->move_group_.move();
-  //}
-
-  waypoints.push_back(msg->pose);
-
-  if (waypoints.size() >= 2)
-  {
-      moveit_msgs::msg::RobotTrajectory trajectory;
-
-      // dangerous with real robot
-      // https://moveit.picknik.ai/galactic/doc/examples/move_group_interface/move_group_interface_tutorial.html
-      const double jump_threshold = 0.0;
-
-      const double eef_step = 0.01;
-      double fraction = this->move_group_.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
-      RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);
-
-      this->move_group_.execute(trajectory);
-
-      waypoints.clear();
-  }
-
-  // Update for next callback
-  previous_target_pose_ = msg->pose;
-  this->moved = true;
-}
-
-int main(int argc, char *argv[])
-{
-  rclcpp::init(argc, argv);
-
-  auto target_follower = std::make_shared<MoveItFollowTarget>();
-
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(target_follower);
-  executor.spin();
-
-  rclcpp::shutdown();
-  return EXIT_SUCCESS;
-}

src/draw_svg/src/cpp/log_position.cpp

@@ -1,76 +0,0 @@
-#include <memory>
-#include <cstdio>
-#include <iostream>
-#include <string>
-
-#include <rclcpp/rclcpp.hpp>
-#include <moveit/move_group_interface/move_group_interface.h>
-//#include <moveit/moveit_commander>
-//https://github.com/AndrejOrsula/ign_moveit2_examples/blob/master/examples/cpp/ex_follow_target.cpp
-const std::string MOVE_GROUP = "lite6";
-
-class PositionLogger : public rclcpp::Node
-{
-public:
-  /// Constructor
-  PositionLogger();
-
-  /// Move group interface for the robot
-  moveit::planning_interface::MoveGroupInterface move_group_;
-  /// Subscriber for target pose
-  //rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
-  /// Target pose that is used to detect changes
-  //geometry_msgs::msg::Pose previous_target_pose_;
-
-private:
-  /// Callback that plans and executes trajectory each time the target pose is changed
-  void log_position();
-};
-
-PositionLogger::PositionLogger() : Node("position_logger"),
-                                   move_group_(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
-{
-  // Use upper joint velocity and acceleration limits
-  this->move_group_.setMaxAccelerationScalingFactor(1.0);
-  this->move_group_.setMaxVelocityScalingFactor(1.0);
-
-  // Subscribe to target pose
-  //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
-  auto ros_clock = rclcpp::Clock::make_shared();
-  auto timer_ = rclcpp::create_timer(this, ros_clock, rclcpp::Duration::from_nanoseconds(5000000),
-   [=]()
-   {
-       this->log_position();
-   });
-
-  RCLCPP_INFO(this->get_logger(), "Initialization successful.");
-}
-
-void PositionLogger::log_position()
-{
-  auto const logger = rclcpp::get_logger("position_logger");
-
-  auto msg = this->move_group_.getCurrentPose();
-  auto p = msg.pose.position;
-  char a[100];
-  std::sprintf(a, "Position x: %f, y: %f, z: %f", p.x,p.y,p.z);
-  std::string s(a);
-
-  //rclcpp::RCLCPP_INFO(logger, a);
-  RCLCPP_INFO(logger, "AA");
-}
-
-int main(int argc, char * argv[])
-{
-  rclcpp::init(argc, argv);
-  // TODO Try moveit_commander::roscpp_initialize
-
-  auto pl = std::make_shared<PositionLogger>();
-
-  rclcpp::executors::SingleThreadedExecutor executor;
-  executor.add_node(pl);
-  executor.spin();
-
-  rclcpp::shutdown();
-  return EXIT_SUCCESS;
-}

src/draw_svg/src/py/draw_svg.py

@@ -1,112 +0,0 @@
-#!/usr/bin/env python3
-"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo"""
-
-import rclpy
-from geometry_msgs.msg import Pose, PoseStamped
-from pymoveit2 import MoveIt2
-from pymoveit2.robots import panda
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.node import Node
-from rclpy.qos import QoSProfile
-from random import uniform as rand
-import math
-#from tf2_ros.transformations import quaternion_from_euler
-import lxml.etree as ET
-
-
-def quaternion_from_euler(ai, aj, ak):
-    ai /= 2.0
-    aj /= 2.0
-    ak /= 2.0
-    ci = math.cos(ai)
-    si = math.sin(ai)
-    cj = math.cos(aj)
-    sj = math.sin(aj)
-    ck = math.cos(ak)
-    sk = math.sin(ak)
-    cc = ci*ck
-    cs = ci*sk
-    sc = si*ck
-    ss = si*sk
-
-    q = [0,0,0,0]
-    q[0] = cj*sc - sj*cs
-    q[1] = cj*ss + sj*cc
-    q[2] = cj*cs - sj*sc
-    q[3] = cj*cc + sj*ss
-
-    return q
-
-def translate(val, lmin, lmax, rmin, rmax):
-    lspan = lmax - lmin
-    rspan = rmax - rmin
-    val = float(val - lmin) / float(lspan)
-    return rmin + (val * rspan)
-
-def map_point_function(x_pixels, y_pixels, xlim_lower, xlim_upper, ylim_lower, ylim_upper):
-    def map_point(xpix,ypix):
-       x = translate(xpix, 0, x_pixels, xlim_lower, xlim_upper)
-       y = translate(ypix, 0, y_pixels, ylim_lower, ylim_upper)
-       return (x,y)
-    return map_point
-
-
-class PublishTarget(Node):
-    def __init__(self):
-        super().__init__('publisher')
-        self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10)
-        timer_period = 4.0  # seconds
-        self.timer = self.create_timer(timer_period, self.timer_callback)
-        self.i = 0
-
-        # TODO get dimensions from svg
-
-        #print(p)
-        #print(p.position)
-        #print(p.orientation)
-        xml = ET.parse('svg/test.svg')
-        svg = xml.getroot()
-        self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.2, 0.4, -0.1, 0.1)
-        self.points = []
-        for child in svg:
-            if (child.tag == 'line'):
-                self.points.append((float(child.get('x1')), float(child.get('y1'))))
-                self.points.append((float(child.get('x2')), float(child.get('y2'))))
-
-
-    def timer_callback(self):
-        next_point = self.points[self.i]
-        point = self.map_point(float(next_point[0]),float(next_point[1]))
-        p = Pose()
-        p.position.x = point[0]
-        p.position.y = point[1]
-        p.position.z = 0.1
-        q = quaternion_from_euler(0.0, math.pi, 0.0)
-        #p.orientation = q
-        p.orientation.x = q[0]
-        p.orientation.y = q[1]
-        p.orientation.z = q[2]
-        p.orientation.w = q[3]
-        ps = PoseStamped()
-        ps.pose = p
-        #print(ps)
-        self.publisher_.publish(ps)
-        self.get_logger().info('Publishing to /target_pose: "%s"' % p)
-        self.i = (self.i + 1) % len(self.points)
-
-def main(args=None):
-    rclpy.init(args=args)
-
-    publisher = PublishTarget()
-
-    rclpy.spin(publisher)
-
-    # Destroy the node explicitly
-    # (optional - otherwise it will be done automatically
-    # when the garbage collector destroys the node object)
-    publisher.destroy_node()
-    rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

src/draw_svg/src/py/drawing_surface.py

@@ -1,207 +0,0 @@
-#!/usr/bin/env python3
-"""GUI for virtual drawing surface"""
-
-import rclpy
-from geometry_msgs.msg import Pose, PoseWithCovariance, Point
-from nav_msgs.msg import Odometry
-from sensor_msgs.msg import Image as SensorImage
-from robots import lite6
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.node import Node
-from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
-
-from queue import Queue
-
-import tkinter as tk
-import math
-
-import threading
-
-from PIL import ImageTk
-from PIL import Image as PImage
-import numpy as np
-
-
-def translate(val, lmin, lmax, rmin, rmax):
-    lspan = lmax - lmin
-    rspan = rmax - rmin
-    val = float(val - lmin) / float(lspan)
-    return rmin + (val * rspan)
-
-class DrawingApp(tk.Tk):
-    def __init__(self, point_queue, image_queue):
-        tk.Tk.__init__(self)
-
-        self.point_queue = point_queue
-        self.image_queue = image_queue
-        self.width = 400
-        self.height = 400
-        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.counter = 0
-        self.read_queue()
-
-        self.canvas = tk.Canvas(self,width=self.width,height=self.height)
-
-
-        self.tk_image = ImageTk.PhotoImage(self.img)
-        self.canvas.create_image(self.width/2,self.height/2,image=self.tk_image)
-        self.canvas.pack(side='top', expand=True, fill='both')
-
-        def reset():
-            self.img = PImage.new("RGB", (self.width, self.height), (255, 255, 255))
-            self.arr = np.array(self.img)
-        tk.Button(self.canvas, text= "reset", command=reset).pack()
-
-        self.draw_window()
-
-    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
-        self.arr[x,y,2] = 0
-        self.arr[x+1,y,1] = 0
-        self.arr[x+1,y,2] = 0
-        self.arr[x+1,y+1,1] = 0
-        self.arr[x+1,y+1,2] = 0
-        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):
-        self.img = PImage.fromarray(self.arr)
-        self.tk_image = ImageTk.PhotoImage(self.img)
-        self.canvas.create_image(self.width/2,self.height/2,image=self.tk_image)
-        self.image_queue.put(self.get_image_message())
-        #self.after(self.window_update_delay, lambda: self.draw_window())
-
-    def read_queue(self):
-        '''Read queue and draw circle every 10 ms'''
-        self.counter = self.counter + self.frame_delay
-        if self.point_queue.empty():
-            self.after(self.frame_delay, lambda: self.read_queue())
-            return
-
-        while not self.point_queue.empty():
-            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)
-            y = translate(p.y, -1.0, 0.5, 0, self.height)
-            self.draw(int(x),int(y),0)
-
-        if self.counter >= self.window_update_delay:
-            #print("Redraw")
-            self.counter = 0
-            self.draw_window()
-
-        self.after(self.frame_delay, lambda: self.read_queue())
-
-    # https://stackoverflow.com/questions/64373334/how-can-i-publish-pil-image-binary-through-ros-without-opencv
-    def get_image_message(self):
-        msg = SensorImage()
-        #msg.header.stamp = rospy.Time.now()
-        msg.height = self.img.height
-        msg.width = self.img.width
-        msg.encoding = "rgb8"
-        msg.is_bigendian = False
-        msg.step = 3 * self.img.width
-        msg.data = np.array(self.img).tobytes()
-        return msg
-
-class LogPen(Node):
-    def __init__(self, point_queue=Queue()):
-
-        self.queue = point_queue
-        super().__init__("log_pen")
-
-        # Create callback group that allows execution of callbacks in parallel without restrictions
-        self._callback_group = ReentrantCallbackGroup()
-
-        self.create_subscription(
-            msg_type=Odometry,
-            topic="/pen_position",
-            callback=self.pen_position_callback,
-            qos_profile=QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT,
-                                   history=HistoryPolicy.KEEP_LAST,
-                                   depth=1),
-            callback_group=self._callback_group,
-        )
-        self.get_logger().info("Initialization successful.")
-    def pen_position_callback(self, msg: Odometry):
-        """
-        Log position of pen
-        """
-        p = msg.pose.pose.position
-        #self.get_logger().info("x:{} y:{} z:{}".format(p.x, p.y, p.z))
-        self.queue.put(p)
-
-class PublishImage(Node):
-    def __init__(self, image_queue=Queue()):
-
-        self.image_queue = image_queue
-        super().__init__("publish_image")
-
-        self.publisher_ = self.create_publisher(SensorImage, '/drawing_surface/image_raw', 10)
-        timer_period = 0.0002  # seconds
-        self.timer = self.create_timer(timer_period, self.timer_callback)
-        self.get_logger().info("Initialization successful.")
-
-    def timer_callback(self):
-        """
-        Output image from queue
-        """
-        if self.image_queue.empty():
-            return
-
-        img = self.image_queue.get()
-        #self.get_logger().info("Publishing image {}x{}".format(img.width,img.height))
-
-        self.publisher_.publish(img)
-
-
-
-def main(args=None):
-
-    rclpy.init(args=args)
-
-    log_pen = LogPen()
-
-    executor = rclpy.executors.MultiThreadedExecutor(2)
-    executor.add_node(log_pen)
-    executor.spin()
-
-    rclpy.shutdown()
-    exit(0)
-
-def start_node_thread(constructor, queue=Queue()):
-
-    node = constructor(queue)
-
-    executor = rclpy.executors.MultiThreadedExecutor(2)
-    executor.add_node(node)
-    executor.spin()
-
-
-if __name__ == "__main__":
-    point_queue = Queue()
-    image_queue = Queue()
-
-    rclpy.init()
-
-    global app
-    app = DrawingApp(point_queue, image_queue)
-
-    global log_thread
-    log_thread = threading.Thread(target=start_node_thread, args=[LogPen, point_queue])
-    log_thread.start()
-
-    global image_thread
-    image_thread = threading.Thread(target=start_node_thread, args=[PublishImage, image_queue])
-    image_thread.start()
-
-    app.mainloop()
-    rclpy.shutdown()
-    exit(0)

src/draw_svg/src/py/follow.py

@@ -1,87 +0,0 @@
-#!/usr/bin/env python3
-"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo"""
-
-import rclpy
-from geometry_msgs.msg import Pose, PoseStamped
-from pymoveit2 import MoveIt2
-from robots import lite6
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.node import Node
-from rclpy.qos import QoSProfile
-
-
-class FollowTarget(Node):
-    def __init__(self):
-
-        super().__init__("follow_py")
-
-        # Create callback group that allows execution of callbacks in parallel without restrictions
-        self._callback_group = ReentrantCallbackGroup()
-
-        # Create MoveIt 2 interface
-        self._moveit2 = MoveIt2(
-            node=self,
-            joint_names=lite6.joint_names(),
-            base_link_name=lite6.base_link_name(),
-            end_effector_name=lite6.end_effector_name(),
-            group_name=lite6.MOVE_GROUP_ARM,
-            execute_via_moveit=True,
-            callback_group=self._callback_group,
-        )
-        # Use upper joint velocity and acceleration limits
-        self._moveit2.max_velocity = 1.0
-        self._moveit2.max_acceleration = 1.0
-
-        # Create a subscriber for target pose
-        p = Pose()
-        p.position.x=0.5
-        p.position.y=0.5
-        self.__previous_target_pose = p
-        self.create_subscription(
-            msg_type=PoseStamped,
-            topic="/target_pose",
-            callback=self.target_pose_callback,
-            qos_profile=QoSProfile(depth=1),
-            callback_group=self._callback_group,
-        )
-
-        self.get_logger().info("Initialization successful.")
-
-    def target_pose_callback(self, msg: PoseStamped):
-        """
-        Plan and execute trajectory each time the target pose is changed
-        """
-
-        # Return if target pose is unchanged
-        if msg.pose == self.__previous_target_pose:
-            return
-
-        self.get_logger().info("Target pose has changed. Planning and executing...")
-
-        # Options https://github.com/AndrejOrsula/pymoveit2/blob/master/pymoveit2/moveit2.py
-        # Plan and execute motion
-        self._moveit2.move_to_pose(
-            position=msg.pose.position,
-            quat_xyzw=msg.pose.orientation,
-            cartesian=True,
-        )
-
-        # Update for next callback
-        self.__previous_target_pose = msg.pose
-
-def main(args=None):
-
-    rclpy.init(args=args)
-
-    target_follower = FollowTarget()
-
-    executor = rclpy.executors.MultiThreadedExecutor(2)
-    executor.add_node(target_follower)
-    executor.spin()
-
-    rclpy.shutdown()
-    exit(0)
-
-
-if __name__ == "__main__":
-    main()

src/draw_svg/src/py/readsvg.py

@@ -1,22 +0,0 @@
-#!/usr/bin/env python3
-
-import lxml.etree as ET
-
-xml = ET.parse('svg/test.svg')
-svg = xml.getroot()
-print(svg)
-
-# AttributeError: 'lxml.etree._ElementTree' object has no attribute 'tag'
-print(svg.tag)
-
-# TypeError: 'lxml.etree._ElementTree' object is not subscriptable
-print(svg[0])
-
-# TypeError: 'lxml.etree._ElementTree' object is not iterable
-for child in svg:
-    print('width:', svg.get('width'))
-    if (child.tag == 'line'):
-        print(child.get('x1'))
-        print(child.get('x2'))
-        print(child.get('y1'))
-        print(child.get('y2'))

src/draw_svg/src/py/robots/lite6.py

@@ -1,35 +0,0 @@
-from typing import List
-
-MOVE_GROUP_ARM: str = "lite6"
-MOVE_GROUP_GRIPPER: str = "gripper"
-
-OPEN_GRIPPER_JOINT_POSITIONS: List[float] = [0.04, 0.04]
-CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = [0.0, 0.0]
-
-# https://github.com/xArm-Developer/xarm_ros2/blob/master/xarm_moveit_config/srdf/_lite6_macro.srdf.xacro
-def joint_names(prefix: str = "") -> List[str]:
-    return [
-        prefix + "world_joint",
-        prefix + "joint1",
-        prefix + "joint2",
-        prefix + "joint3",
-        prefix + "joint4",
-        prefix + "joint5",
-        prefix + "joint6",
-        prefix + "joint_eef",
-    ]
-
-
-def base_link_name(prefix: str = "") -> str:
-    return prefix + "link_base"
-
-
-def end_effector_name(prefix: str = "") -> str:
-    return prefix + "link_eef"
-
-
-def gripper_joint_names(prefix: str = "") -> List[str]:
-    return [
-        prefix + "link_eef",
-        prefix + "link_eef",
-    ]

src/draw_svg/svg/test.svg

@@ -1,7 +0,0 @@
-<svg height="210" width="500">
-  <line x1="100" y1="200" x2="250" y2="10" style="stroke:rgb(255,0,0);stroke-width:2" />
-  <line x1="250" y1="10" x2="400" y2="200" style="stroke:rgb(255,0,0);stroke-width:2" />
-  <line x1="400" y1="200" x2="20" y2="90" style="stroke:rgb(255,0,0);stroke-width:2" />
-  <line x1="20" y1="90" x2="450" y2="90" style="stroke:rgb(255,0,0);stroke-width:2" />
-  <line x1="450" y1="90" x2="100" y2="200" style="stroke:rgb(255,0,0);stroke-width:2" />
-</svg>

src/draw_svg/test.sh

@@ -1 +0,0 @@
-ros2 topic pub --once /target_pose geometry_msgs/msg/PoseStamped '{header:{}, pose:{position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {}}}'

src/draw_svg/urdf/lite6.tmp.urdf

@@ -1,436 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from urdf/xarm_pen.urdf.xacro       | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="xarm6">
-  <gazebo>
-    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
-      <parameters>/root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml</parameters>
-    </plugin>
-  </gazebo>
-  <link name="world"/>
-  <joint name="world_joint" type="fixed">
-    <parent link="world"/>
-    <child link="link_base"/>
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-  </joint>
-  <ros2_control name="UFRobotSystem" type="system">
-    <hardware>
-      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
-      <param name="hw_ns">xarm</param>
-      <param name="velocity_control">False</param>
-      <param name="prefix">P</param>
-      <param name="robot_ip">R</param>
-      <param name="report_type">normal</param>
-      <param name="dof">6</param>
-      <param name="baud_checkset">True</param>
-      <param name="default_gripper_baud">2000000</param>
-      <param name="robot_type">lite</param>
-      <param name="add_gripper">False</param>
-    </hardware>
-    <joint name="joint1">
-      <command_interface name="position">
-        <param name="min">-6.283185307179586</param>
-        <param name="max">6.283185307179586</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-3.14</param>
-        <param name="max">3.14</param>
-      </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <!-- <state_interface name="effort"/> -->
-    </joint>
-    <joint name="joint2">
-      <command_interface name="position">
-        <param name="min">-2.61799</param>
-        <param name="max">2.61799</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-3.14</param>
-        <param name="max">3.14</param>
-      </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <!-- <state_interface name="effort"/> -->
-    </joint>
-    <joint name="joint3">
-      <command_interface name="position">
-        <param name="min">-0.061087</param>
-        <param name="max">5.235988</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-3.14</param>
-        <param name="max">3.14</param>
-      </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <!-- <state_interface name="effort"/> -->
-    </joint>
-    <joint name="joint4">
-      <command_interface name="position">
-        <param name="min">-6.283185307179586</param>
-        <param name="max">6.283185307179586</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-3.14</param>
-        <param name="max">3.14</param>
-      </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <!-- <state_interface name="effort"/> -->
-    </joint>
-    <joint name="joint5">
-      <command_interface name="position">
-        <param name="min">-2.1642</param>
-        <param name="max">2.1642</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-3.14</param>
-        <param name="max">3.14</param>
-      </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <!-- <state_interface name="effort"/> -->
-    </joint>
-    <joint name="joint6">
-      <command_interface name="position">
-        <param name="min">-6.283185307179586</param>
-        <param name="max">6.283185307179586</param>
-      </command_interface>
-      <command_interface name="velocity">
-        <param name="min">-3.14</param>
-        <param name="max">3.14</param>
-      </command_interface>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <!-- <state_interface name="effort"/> -->
-    </joint>
-  </ros2_control>
-  <material name="White">
-    <color rgba="1.0 1.0 1.0 1.0"/>
-  </material>
-  <material name="Silver">
-    <color rgba="0.753 0.753 0.753 1.0"/>
-  </material>
-  <link name="link_base">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00829544579053192 3.26357432323433E-05 0.0631194584987089"/>
-      <mass value="1.65393501783165"/>
-      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="link1">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.00036 0.03788 -0.0027"/>
-      <mass value="1.169"/>
-      <inertia ixx="1.45164E-03" ixy="1.24E-05" ixz="-6.7E-06" iyy="8.873E-04" iyz="1.255E-04" izz="1.31993E-03"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="joint1" type="revolute">
-    <origin rpy="0 0 0" xyz="0 0 0.2435"/>
-    <parent link="link_base"/>
-    <child link="link1"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="50.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
-    <dynamics damping="1.0" friction="1.0"/>
-  </joint>
-  <link name="link2">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.178 0.0 0.0576"/>
-      <mass value="1.192"/>
-      <inertia ixx="1.5854E-03" ixy="-6.766E-06" ixz="-1.15136E-03" iyy="5.6097E-03" iyz="1.14E-06" izz="4.85E-03"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="joint2" type="revolute">
-    <origin rpy="1.5708 -1.5708 3.1416" xyz="0 0 0"/>
-    <parent link="link1"/>
-    <child link="link2"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="50.0" lower="-2.61799" upper="2.61799" velocity="3.14"/>
-    <dynamics damping="1.0" friction="1.0"/>
-  </joint>
-  <link name="link3">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.07285 -0.030 -0.0009"/>
-      <mass value="0.930"/>
-      <inertia ixx="8.861E-04" ixy="-3.9287E-04" ixz="7.066E-05" iyy="1.5785E-03" iyz="-2.445E-05" izz="1.84677E-03"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="joint3" type="revolute">
-    <origin rpy="-3.1416 0 1.5708" xyz="0.2002 0 0"/>
-    <parent link="link2"/>
-    <child link="link3"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="32.0" lower="-0.061087" upper="5.235988" velocity="3.14"/>
-    <dynamics damping="1.0" friction="1.0"/>
-  </joint>
-  <link name="link4">
-    <inertial>
-      <origin rpy="0 0 0" xyz="-0.0004 -0.0275 -0.0817"/>
-      <mass value="1.31"/>
-      <inertia ixx="3.705E-03" ixy="-2.0E-06" ixz="7.17E-06" iyy="3.0455E-03" iyz="-9.3188E-04" izz="1.5413E-03"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="joint4" type="revolute">
-    <origin rpy="1.5708 0 0" xyz="0.087 -0.22761 0"/>
-    <parent link="link3"/>
-    <child link="link4"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="32.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
-    <dynamics damping="1.0" friction="1.0"/>
-  </joint>
-  <link name="link5">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 0.010 0.0019"/>
-      <mass value="0.784"/>
-      <inertia ixx="5.668E-04" ixy="6E-07" ixz="-5.3E-06" iyy="5.077E-04" iyz="-4.8E-07" izz="5.3E-04"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
-      </geometry>
-      <material name="White"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="joint5" type="revolute">
-    <origin rpy="1.5708 0 0" xyz="0 0 0"/>
-    <parent link="link4"/>
-    <child link="link5"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="32.0" lower="-2.1642" upper="2.1642" velocity="3.14"/>
-    <dynamics damping="1.0" friction="1.0"/>
-  </joint>
-  <link name="link6">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0.0 -0.00194 -0.0102"/>
-      <mass value="0.180"/>
-      <inertia ixx="7.726E-05" ixy="1E-06" ixz="4E-07" iyy="8.5665E-05" iyz="-6E-07" izz="1.4814E-04"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
-      </geometry>
-      <material name="Silver"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
-      </geometry>
-    </collision>
-  </link>
-  <joint name="joint6" type="revolute">
-    <origin rpy="-1.5708 0 0" xyz="0 0.0625 0"/>
-    <parent link="link5"/>
-    <child link="link6"/>
-    <axis xyz="0 0 1"/>
-    <limit effort="20.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
-    <dynamics damping="1.0" friction="1.0"/>
-  </joint>
-  <link name="link_eef"/>
-  <joint name="joint_eef" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="link6"/>
-    <child link="link_eef"/>
-  </joint>
-  <transmission name="tran1">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="joint1">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="motor1">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      <mechanicalReduction>100</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="tran2">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="joint2">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="motor2">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      <mechanicalReduction>100</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="tran3">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="joint3">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="motor3">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      <mechanicalReduction>100</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="tran4">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="joint4">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="motor3">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      <mechanicalReduction>100</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="tran5">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="joint5">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="motor5">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      <mechanicalReduction>100</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <transmission name="tran6">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="joint6">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="motor6">
-      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
-      <mechanicalReduction>100</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <gazebo reference="link_base">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <gazebo reference="link1">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <gazebo reference="link2">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <gazebo reference="link3">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <gazebo reference="link4">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <gazebo reference="link5">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <gazebo reference="link6">
-    <selfCollide>true</selfCollide>
-  </gazebo>
-  <joint name="eef_joint" type="fixed">
-    <parent link="link_eef"/>
-    <child link="pen_link"/>
-  </joint>
-  <link name="pen_link">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <cylinder length="0.2" radius="0.005"/>
-      </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>
-  <!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
-  <gazebo>
-    <plugin filename="libgazebo_ros_p3d.so" name="pen_position">
-      <!-- <alwaysOn>true</alwaysOn> -->
-      <ros>
-        <remapping>odom:=pen_position</remapping>
-      </ros>
-      <frame_name>world</frame_name>
-      <!-- <body_name>pen_link</body_name> -->
-      <body_name>link6</body_name>
-      <!--       topic name is always /odom -->
-      <!-- <topic_name>pen_position</topic_name> -->
-      <!--       Update rate in Hz -->
-      <update_rate>10</update_rate>
-      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
-      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
-    </plugin>
-  </gazebo>
-</robot>
-<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
-     <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
-     <xacro:lite6_urdf prefix=''/> -->
-<!--
-     </robot> -->

src/draw_svg/urdf/xarm_pen.urdf.xacro

@@ -1,100 +0,0 @@
-<?xml version='1.0' encoding='utf-8'?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm$(arg dof)">
-    <!-- <robot xmlns:xacro="http://ros.org/wiki/xacro" name="lite6"> -->
-  <xacro:arg name="prefix" default=""/>
-  <xacro:arg name="hw_ns" default="xarm"/>
-  <xacro:arg name="limited" default="false"/>
-  <xacro:arg name="effort_control" default="false"/>
-  <xacro:arg name="velocity_control" default="false"/>
-  <xacro:arg name="add_gripper" default="false"/>
-  <xacro:arg name="add_vacuum_gripper" default="false"/>
-  <xacro:arg name="dof" default="6"/>
-  <xacro:arg name="robot_type" default="lite"/>
-  <xacro:arg name="ros2_control_plugin" default="uf_robot_hardware/UFRobotSystemHardware"/>
-  <xacro:arg name="ros2_control_params" default="$(find xarm_controller)/config/xarm$(arg dof)_controllers.yaml"/>
-
-  <xacro:arg name="add_other_geometry" default="false"/>
-  <xacro:arg name="geometry_type" default="box"/>
-  <xacro:arg name="geometry_mass" default="0.1"/>
-  <xacro:arg name="geometry_height" default="0.1"/>
-  <xacro:arg name="geometry_radius" default="0.1"/>
-  <xacro:arg name="geometry_length" default="0.1"/>
-  <xacro:arg name="geometry_width" default="0.1"/>
-  <xacro:arg name="geometry_mesh_filename" default=""/>
-  <xacro:arg name="geometry_mesh_origin_xyz" default="0 0 0"/>
-  <xacro:arg name="geometry_mesh_origin_rpy" default="0 0 0"/>
-  <xacro:arg name="geometry_mesh_tcp_xyz" default="0 0 0"/>
-  <xacro:arg name="geometry_mesh_tcp_rpy" default="0 0 0"/>
-
-  <xacro:arg name="robot_ip" default=""/>
-  <xacro:arg name="report_type" default="normal"/>
-  <xacro:arg name="baud_checkset" default="true"/>
-  <xacro:arg name="default_gripper_baud" default="2000000"/>
-
-  <!-- gazebo_plugin -->
-  <xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
-  <xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/>
-
-  <!-- load xarm device -->
-  <!-- Load Lite6 Robot Model URDF -->
-  <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6_robot_macro.xacro" />
-  <xacro:lite6_robot prefix="$(arg prefix)" namespace="xarm" limited="$(arg limited)"
-                     effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
-                     ros2_control_plugin="$(arg ros2_control_plugin)"
-                     attach_to="world" xyz="0 0 0" rpy="0 0 0"
-                     ros2_control_params="$(arg ros2_control_params)"
-                     load_gazebo_plugin="false"
-                     add_gripper="$(arg add_gripper)"
-                     robot_ip="$(arg robot_ip)"
-                     report_type="$(arg report_type)"
-                     baud_checkset="$(arg baud_checkset)"
-                     default_gripper_baud="$(arg default_gripper_baud)" />
-
-  <joint name="eef_joint" type="fixed">
-      <parent link="link_eef"/>
-      <child link="pen_link"/>
-  </joint>
-  <link name="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>
-
-  <!-- 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>link6</body_name>
-      <!--       topic name is always /odom -->
-      <!-- <topic_name>pen_position</topic_name> -->
-      <!--       Update rate in Hz -->
-      <update_rate>10</update_rate>
-      <!-- <xyzOffsets>0 0 0</xyzOffsets> -->
-      <!-- <rpyOffsets>0 0 0</rpyOffsets> -->
-    </plugin>
-  </gazebo>
-
-</robot>
-<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
-     <xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
-     <xacro:lite6_urdf prefix=''/> -->
-
-<!--
-     </robot> -->

src/drawing_controller/drawing_controller/drawing_controller.py

@@ -17,6 +17,8 @@ from robot_interfaces.msg import Motion
 import sys
 from copy import deepcopy
 
+import time
+
 from drawing_controller.svg_processor import SVGProcessor
 
 def quaternion_from_euler(ai, aj, ak):
@@ -61,13 +63,17 @@ class DrawingController(Node):
     def __init__(self, svgpath):
         super().__init__('drawing_controller')
         #self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10)
-        timer_period = 1.0  # seconds
+        timer_period = 0.1  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
         self.i = 0
         self.busy = False
 
         self._action_client = ActionClient(self, ExecuteMotion, 'execute_motion')
 
+        self.results = {}
+
+        self.svg_start_time = time.time()
+
         xml = ET.parse(svgpath)
         svg = xml.getroot()
         #self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2)
@@ -85,7 +91,11 @@ class DrawingController(Node):
 
         self.svg_processor = SVGProcessor(self.get_logger())
         self.svg = self.svg_processor.process_svg(svgpath)
-        self.get_logger().info('Ready to begin executing motions')
+        self.results['svg path'] = svgpath
+
+        self.results['svg processing time'] = self.timestr(time.time() - self.svg_start_time)
+        self.start_time = time.time()
+        self.get_logger().info('SVG processing finished, executing motions')
 
     def send_goal(self, motion):
         self.busy = True
@@ -101,22 +111,23 @@ class DrawingController(Node):
     def goal_response_callback(self, future):
         goal_handle = future.result()
         if not goal_handle.accepted:
-            self.get_logger().info('Goal rejected :(')
+            self.get_logger().debug('Goal rejected :(')
             return
 
-        self.get_logger().info('Goal accepted :)')
+        self.get_logger().debug('Goal accepted :)')
 
         self._get_result_future = goal_handle.get_result_async()
         self._get_result_future.add_done_callback(self.get_result_callback)
 
     def get_result_callback(self, future):
         result = future.result().result
-        self.get_logger().info('Result: {0}'.format(result))
+        self.results.update({ result.result: self.results.get(result.result, 0) + 1 })
+        self.get_logger().info('Result: {0}'.format(result.result))
         self.busy = False
 
     def feedback_callback(self, feedback_msg):
         feedback = feedback_msg.feedback
-        self.get_logger().info('Received feedback: {0}'.format(feedback))
+        self.get_logger().debug('Received feedback: {0}'.format(feedback))
 
     def append_points(self, motion, points):
         for point in points:
@@ -135,17 +146,23 @@ class DrawingController(Node):
             ps.pose = p
             motion.path.append(ps)
 
+    def timestr(self, t):
+        return time.strftime("%H:%M:%S", time.gmtime(t))
+
     def timer_callback(self):
         if self.busy:
             return
         if self.i >= len(self.svg):
+            self.results['total time'] = self.timestr(time.time() - self.svg_start_time)
+            self.results['drawing time'] = self.timestr(time.time() - self.start_time)
             self.get_logger().info('Finished executing all motions from SVG')
+            self.get_logger().info('Results: {}'.format(self.results))
             exit()
         next_motion = self.svg[self.i]
         motion = Motion()
         self.append_points(motion, next_motion)
         self.i = self.i + 1
-        self.get_logger().info('Executing motion: {}...'.format(motion.path[:10]))
+        self.get_logger().info('Executing motion: {}/{}'.format(self.i, len(self.svg)))
         self.send_goal(motion)
 
 

src/drawing_controller/drawing_controller/svg_path_parser.py

@@ -11,7 +11,7 @@ class SVGPathParser():
         self.map_point = map_point
 
     def tokenize(self, pathstr):
-        self.logger.info("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr)))
+        self.logger.debug("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr)))
         path = []
         i = 0
         while i < len(pathstr):
@@ -53,7 +53,7 @@ class SVGPathParser():
             Returns:
                     primitive_fn ():
         '''
-        self.logger.info("Parsing path :'{}...' with {} tokens".format(path[:20], len(path)))
+        self.logger.debug("Parsing path :'{}...' with {} tokens".format(path[:20], len(path)))
         x = 0.0
         y = 0.0
         i = 0
@@ -114,13 +114,13 @@ class SVGPathParser():
             #print('inpput', control_points)
             maxval = np.amax(np.absolute(control_points))
             #print('maxxv', maxval)
-            control_points = control_points / maxval #normalize values
+            #control_points = control_points / maxval #normalize values
-            n = 50
+            n = 100
-            curve = cf.cubic_curve(control_points)
+            curve = cf.bezier(control_points)
             lin = np.linspace(curve.start(0), curve.end(0), n)
             coordinates = curve(lin)
             coordinates = np.nan_to_num(coordinates)
-            coordinates = coordinates * maxval #denormalize values
+            #coordinates = coordinates * maxval #denormalize values
             coordinates = dropzeros(coordinates)
             #self.logger.info("Appending curve points: {}".format(coordinates))
             #print(coordinates)
@@ -136,6 +136,20 @@ class SVGPathParser():
                 x = coordinates[-1][0]
                 y = coordinates[-1][1]
             return coordinates
+        def quadratic_bezier(control_points):
+            nonlocal x
+            nonlocal y
+            control_points = np.array(control_points)
+            n = 100
+            curve = cf.bezier(control_points, quadratic=True)
+            lin = np.linspace(curve.start(0), curve.end(0), n)
+            coordinates = curve(lin)
+            coordinates = np.nan_to_num(coordinates)
+            coordinates = dropzeros(coordinates)
+            if len(coordinates) > 0:
+                x = coordinates[-1][0]
+                y = coordinates[-1][1]
+            return coordinates
 
         while i < len(path):
             w = path[i]
@@ -208,7 +222,6 @@ class SVGPathParser():
             # Cubic Bézier Curve commands
             if (w == "C"):
                 while True:
-                    # https://github.com/sintef/Splipy/tree/master/examples
                     control_points = [(x,y),
                                       (getnum(),getnum()),
                                       (getnum(),getnum()),
@@ -221,7 +234,6 @@ class SVGPathParser():
                 continue
             if (w == "c"):
                 while True:
-                    # https://github.com/sintef/Splipy/tree/master/examples
                     control_points = [(x,y),
                                       (x + getnum(), y + getnum()),
                                       (x + getnum(), y + getnum()),
@@ -238,9 +250,27 @@ class SVGPathParser():
                 self.logger.error("SVG path parser '{}' not implemented".format(w))
             # Quadratic Bézier Curve commands
             if (w == "Q"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    control_points = [(x,y),
+                                      (getnum(),getnum()),
+                                      (getnum(),getnum())]
+                    coordinates = quadratic_bezier(control_points)
+                    appendpoints(coordinates)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "q"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    control_points = [(x,y),
+                                      (x + getnum(), y + getnum()),
+                                      (x + getnum(), y + getnum())]
+                    coordinates = quadratic_bezier(control_points)
+                    appendpoints(coordinates)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "T"):
                 self.logger.error("SVG path parser '{}' not implemented".format(w))
             if (w == "t"):
@@ -259,5 +289,5 @@ class SVGPathParser():
             self.logger.error("SVG path parser panic mode at '{}'".format(w))
 
             i += 1
-        self.logger.info("Finished parsing path :'{}...' with {} points".format(output[:3], len(output)))
+        self.logger.debug("Finished parsing path :'{}...' with {} points".format(output[:3], len(output)))
         return output

src/drawing_controller/drawing_controller/svg_processor.py

@@ -174,7 +174,7 @@ class SVGProcessor():
     def remove_redundant(self, motion):
         # Remove points that are too close to the previous point, specified by the tolerance
         mm = []
-        tolerance = 0.001
+        tolerance = 0.0001
         prev = (-1, -1, 0)
         for i, p in enumerate(motion):
             x = p[0]
@@ -197,8 +197,7 @@ class SVGProcessor():
         Simplify line with https://pypi.org/project/simplification/
         """
         # For RDP, Try an epsilon of 1.0 to start with. Other sensible values include 0.01, 0.001
-        #epsilon = 0.009
+        epsilon =  0.00005
-        epsilon = 0.005
 
         tmp = []
         out = []
@@ -215,6 +214,7 @@ class SVGProcessor():
                 tmp.append(list(p)[:-1])
             lastup = penup
 
+        # Handle anything left in tmp
         if (len(tmp) > 0):
             out += sf(tmp)
 

src/lite6_controller/CMakeLists.txt

@@ -16,6 +16,8 @@ find_package(moveit REQUIRED)
 find_package(pilz_industrial_motion_planner REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(xarm_api REQUIRED)
+find_package(xarm_msgs REQUIRED)
 
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -45,6 +47,19 @@ ament_target_dependencies(lite6_controller
   "moveit_ros_planning_interface"
   "robot_interfaces")
 
+add_executable(lite6_calibration src/lite6_calibration.cpp)
+ament_target_dependencies(lite6_calibration
+  "xarm_msgs"
+  "xarm_api"
+  "rclcpp"
+  "moveit"
+  "rclcpp_action"
+  "Eigen3"
+  "pilz_industrial_motion_planner"
+  "robot_controller"
+  "moveit_ros_planning_interface"
+  "robot_interfaces")
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
@@ -52,8 +67,14 @@ endif()
 
 install(TARGETS
   lite6_controller
+  lite6_calibration
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_package()

src/lite6_controller/config/config.yaml

@@ -0,0 +1,13 @@
+/robot_controller:
+  ros__parameters:
+    lite6_x_limit_lower: 0.1
+    lite6_x_limit_upper: 0.305
+    lite6_y_limit_lower: -0.1475
+    lite6_y_limit_upper: 0.1475
+
+    lite6_z_lift_amount: 0.01
+    lite6_z_offset: 0.1475
+
+    lite6_blend_radius: 0.0
+    lite6_max_velocity_scaling_factor: 1.0
+    lite6_max_acceleration_scaling_factor: 0.9

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -2,6 +2,7 @@
 """Launch Python example for following a target"""
 
 import os
+import yaml
 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
@@ -17,7 +18,8 @@ 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"))
+    rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("custom_xarm_description"), "rviz", "display.rviz"))
+
 
     prefix = LaunchConfiguration('prefix', default='')
     hw_ns = LaunchConfiguration('hw_ns', default='xarm')
@@ -54,6 +56,8 @@ def launch_setup(context, *args, **kwargs):
     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')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('lite6_controller'), 'config', 'config.yaml']))
+
     # robot moveit common launch
     # xarm_moveit_config/launch/_robot_moveit_common.launch.py
     robot_moveit_common_launch = IncludeLaunchDescription(
@@ -94,7 +98,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,
@@ -110,108 +115,71 @@ def launch_setup(context, *args, **kwargs):
         }.items(),
     )
 
-    # URDF
+
-    _robot_description_xml = Command(
+    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'))
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
+    get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
-            " ",
+    robot_description = {
-            PathJoinSubstitution(
+        'robot_description': get_xacro_file_content(
-                [FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.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,
+                '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,
+            }
         ),
-            #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
+        'robot_description_semantic': get_xacro_file_content(
+            xacro_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.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,
+            }
+        ),
     }
 
 
@@ -228,7 +196,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],
@@ -299,10 +267,20 @@ def launch_setup(context, *args, **kwargs):
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        lite6_config = yaml.safe_load(f)
+        print(f'Loaded configuration: {lite6_config}')
+    robot_description_parameters.update(lite6_config['/robot_controller']['ros__parameters'])
+
+    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
+    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
+    #input(joint_limits_yaml)
+
     # FIX acceleration limits
-    for i in range(1,7):
+    #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)]['has_acceleration_limits'] = True
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    #    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'
@@ -336,12 +314,8 @@ def launch_setup(context, *args, **kwargs):
         prefix=prefix.perform(context))
 
 
-    robot_description_parameters['cartesian_limits'] = {}
+    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
-    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
@@ -418,15 +392,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
+        'publish_robot_description': True,
-        #     "name": "planning_scene_monitor",
+        'publish_robot_description_semantic' :True,
-        #     "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

src/lite6_controller/launch/lite6_real.launch.py

@@ -1,4 +1,5 @@
 import os
+import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -15,6 +16,7 @@ from launch.events import Shutdown
 
 
 def launch_setup(context, *args, **kwargs):
+
     robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
     report_type = LaunchConfiguration('report_type', default='normal')
     prefix = LaunchConfiguration('prefix', default='')
@@ -55,6 +57,8 @@ def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration('use_sim_time', default=False)
     log_level = LaunchConfiguration("log_level", default='warn')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('lite6_controller'), 'config', 'config.yaml']))
+
     # # robot driver launch
     # # xarm_api/launch/_robot_driver.launch.py
     # robot_driver_launch = IncludeLaunchDescription(
@@ -75,7 +79,7 @@ def launch_setup(context, *args, **kwargs):
     # robot description launch
     # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +109,12 @@ def launch_setup(context, *args, **kwargs):
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     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('/'),
@@ -154,21 +158,22 @@ 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']
 
-    robot_description_parameters['cartesian_limits'] = {}
+    kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
-    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
-    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    kinematics_yaml['kinematics_solver_timeout'] = 0.005
-    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    kinematics_yaml['kinematics_solver_attempts'] = 3
-    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
-
-    #kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
-    #kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
-    #kinematics_yaml['kinematics_solver_timeout'] = 0.005
-    #kinematics_yaml['kinematics_solver_attempts'] = 3
     kinematics_yaml['kinematics_solver_timeout'] = 10.0
+
     kinematics_yaml['kinematics_solver_attempts'] = 10
 
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        lite6_config = yaml.safe_load(f)
+        print(f'Loaded configuration: {lite6_config}')
+    robot_description_parameters.update(lite6_config['/robot_controller']['ros__parameters'])
+
     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')
@@ -186,11 +191,11 @@ def launch_setup(context, *args, **kwargs):
             joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
 
     # FIX acceleration limits
-    for i in range(1,7):
+    #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)]['has_acceleration_limits'] = True
-        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5
+    #    #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.5
-        #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5
+    #    #joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 2.5
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
+    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
 
     add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
     add_prefix_to_moveit_params(
@@ -232,15 +237,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
+        'publish_robot_description': True,
-        #     "name": "planning_scene_monitor",
+        'publish_robot_description_semantic' :True,
-        #     "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,
-        # },
     }
 
 
@@ -249,7 +247,8 @@ def launch_setup(context, *args, **kwargs):
             'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
             #'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""",
             'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            "default_planner_config": "PTP",
+            #"default_planner_config": "PTP",
+            "default_planner_config": "LIN",
         }
     }
 
@@ -264,6 +263,7 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
+            #cartesian_limits,
             #ompl_planning_pipeline_config,
             pilz_planning_pipeline_config,
             move_group_capabilities,

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -1,4 +1,5 @@
 import os
+import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -15,6 +16,7 @@ from launch.events import Shutdown
 
 
 def launch_setup(context, *args, **kwargs):
+
     robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150')
     report_type = LaunchConfiguration('report_type', default='normal')
     prefix = LaunchConfiguration('prefix', default='')
@@ -55,6 +57,8 @@ def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration('use_sim_time', default=False)
     log_level = LaunchConfiguration("log_level", default='warn')
 
+    config = LaunchConfiguration("config", default=PathJoinSubstitution([FindPackageShare('lite6_controller'), 'config', 'config.yaml']))
+
     # # robot driver launch
     # # xarm_api/launch/_robot_driver.launch.py
     # robot_driver_launch = IncludeLaunchDescription(
@@ -75,7 +79,7 @@ def launch_setup(context, *args, **kwargs):
     # robot description launch
     # xarm_description/launch/_robot_description.launch.py
     robot_description_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +109,12 @@ def launch_setup(context, *args, **kwargs):
 
     # robot_description_parameters
     # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     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('/'),
@@ -155,10 +159,16 @@ def launch_setup(context, *args, **kwargs):
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
     joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
 
+    robotcontroller_config = config.perform(context)
+    with open(robotcontroller_config, 'r') as f:
+        lite6_config = yaml.safe_load(f)
+        print(f'Loaded configuration: {lite6_config}')
+    robot_description_parameters.update(lite6_config['/robot_controller']['ros__parameters'])
+
     # FIX acceleration limits
-    for i in range(1,7):
+    #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)]['has_acceleration_limits'] = True
-        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
 
     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)))
@@ -182,11 +192,8 @@ def launch_setup(context, *args, **kwargs):
         kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
         prefix=prefix.perform(context))
 
-    robot_description_parameters['cartesian_limits'] = {}
+    #cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
-    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    #robot_description_parameters['robot_description_planning']['cartesian_limits'] = cartesian_limits['cartesian_limits']
-    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 Configuration
     ompl_planning_pipeline_config = {
@@ -236,15 +243,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
+        'publish_robot_description': True,
-        #     "name": "planning_scene_monitor",
+        'publish_robot_description_semantic' :True,
-        #     "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

src/lite6_controller/package.xml

@@ -15,6 +15,8 @@
   <!-- <depend>robot_controller</depend> -->
   <depend>moveit</depend>
   <depend>moveit_msgs</depend>
+  <depend>xarm_msgs</depend>
+  <depend>xarm_api</depend>
   <depend>geometry_msgs</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>tf2_ros</depend>

src/lite6_controller/src/lite6_calibration.cpp

@@ -0,0 +1,131 @@
+#include <signal.h>
+#include <chrono>
+#include <rclcpp/rclcpp.hpp>
+#include <xarm_api/xarm_ros_client.h>
+#include <iostream>
+#include <string>
+
+// MoveIt
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+
+
+const std::string MOVE_GROUP = "lite6";
+
+std::shared_ptr<rclcpp::Node> node;
+
+void exit_sig_handler(int signum)
+{
+    fprintf(stderr, "[lite6_calibration] Ctrl-C caught, exit process...\n");
+    exit(-1);
+}
+
+void wait()
+{
+ do
+ {
+   std::cout << "Press a key to continue...";
+ } while (std::cin.get() != '\n');
+}
+
+void println(std::string s)
+{
+  std::cout << s << std::endl;
+}
+
+void print_joints(std::vector<float> jnts)
+{
+    std::string out = "";
+    out = out + "{ ";
+    for (auto i : jnts)
+      out = out + std::to_string(i) + ", ";
+    out = out.substr(0, out.size()-2); //remove trailing comma
+    out = out + " }";
+    std::cout << out << std::endl;
+}
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    std::string hw_ns = "ufactory";
+    node = rclcpp::Node::make_shared("lite6_calibration");
+    int ret;
+
+    signal(SIGINT, exit_sig_handler);
+
+    xarm_api::XArmROSClient client;
+    client.init(node, hw_ns);
+    client.motion_enable(true);
+
+    println("Setting xArm lite6 to free-drive mode. Attach pen and touch the drawing surface. (You can also skip this if you do not want to measure Z)");
+    client.set_mode(2);
+    client.set_state(0);
+    //rclcpp::sleep_for(std::chrono::seconds(20));
+    wait();
+    //client.set_mode(0);
+    //client.set_state(0);
+    //rclcpp::sleep_for(std::chrono::seconds(2));
+
+    client.set_mode(0);
+    client.set_state(0);
+    rclcpp::sleep_for(std::chrono::seconds(2));
+
+    //std::vector<float> jnt_drawing_pose = { -0.975004, 0.0734182, 0.443928, 3.14102, -0.370552, -0.85577, 0 };
+    std::vector<float> jnt_drawing_pose = { -0.975008, 0.00889134, 0.453255, 3.1414, -0.444295, -0.85692, 0 } ;
+
+    std::vector<float> jnt_pose = { 0, 0, 0, 0, 0, 0, 0 };
+    client.get_servo_angle(jnt_pose);
+
+    print_joints(jnt_pose);
+
+
+
+    //client.set_servo_angle_j(jnt_drawing_pose, 1, 1, 100);
+    //rclcpp::sleep_for(std::chrono::seconds(5));
+
+
+    // Compute position of pen from joint state
+    robot_model_loader::RobotModelLoader robot_model_loader(node);
+    const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel();
+    moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model));
+    const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup(MOVE_GROUP);
+    std::string ee_link = "pen_link";
+
+    std::vector<double> jnts;
+    for (auto j : jnt_pose)
+        jnts.push_back(j);
+
+    jnts.resize(joint_model_group->getVariableNames().size());
+    kinematic_state->setJointGroupPositions(joint_model_group, jnts);
+
+    // from tutorial https://ros-planning.github.io/moveit_tutorials/doc/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html
+    // https://github.com/ros-planning/moveit_tutorials/blob/master/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp
+    //robot_model_loader::RobotModelLoader robot_model_loader(node, "robot_description");
+
+    kinematic_state->setJointGroupPositions(joint_model_group, jnts);
+    // https://mycourses.aalto.fi/pluginfile.php/1433193/mod_resource/content/2/Intro_to_ROS_Eigen.pdf
+    const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform(ee_link);
+
+    auto x = std::to_string(end_effector_state.translation().x());
+    auto y = std::to_string(end_effector_state.translation().y());
+    auto z = std::to_string(end_effector_state.translation().z());
+    //println("x for '" + ee_link + "': " + x);
+    //println("y for '" + ee_link + "': " + y);
+    println("z-offset value for '" + ee_link + "' (use this value for the current pen): " + z);
+
+
+    println("WARNING. Moving to start drawing pose. Press ctrl-c to cancel");
+    wait();
+    client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1);
+
+
+    client.set_mode(0);
+    client.set_state(0);
+
+    rclcpp::shutdown();
+
+    println("Done, ignore any errors after this. Restart lite6_controller after I exit.");
+    wait();
+    return 0;
+}

src/lite6_controller/src/lite6_controller.cpp

@@ -3,7 +3,7 @@
 #include <robot_controller/robot_controller.hpp>
 #include <chrono>
 #include <cstdlib>
-//#include <queue>
+#include <queue>
 
 #include <fstream>
 #include <iostream>
@@ -28,6 +28,7 @@
 
 
 #include <moveit_msgs/msg/constraints.hpp>
+#include <moveit_msgs/msg/robot_trajectory.hpp>
 #include <moveit_msgs/msg/motion_plan_request.hpp>
 #include <moveit_msgs/msg/motion_sequence_request.hpp>
 #include <moveit_msgs/msg/motion_sequence_item.hpp>
@@ -46,15 +47,6 @@ const std::string MOVE_GROUP = "lite6";
 
 using namespace std::chrono_literals;
 
-// MOTION PLANNING API
-// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp
-// https://moveit.picknik.ai/humble/doc/examples/motion_planning_api/motion_planning_api_tutorial.html
-// https://github.com/ros-planning/moveit2/blob/main/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp
-//
-//
-// USE
-// https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
-
 /**
  * Controller for xArm Lite6, implementing RobotController
  */
@@ -65,6 +57,11 @@ public:
    * Move group interface for the robot
    */
   moveit::planning_interface::MoveGroupInterface move_group;
+  moveit::core::RobotStatePtr move_group_state;
+
+  std::queue<moveit_msgs::msg::RobotTrajectory> trajectory_queue;
+  rclcpp::TimerBase::SharedPtr trajectory_timer_;
+  bool busy = false;
 
   /**
    * TODO Use instead of MoveGroupInterface
@@ -83,20 +80,9 @@ public:
   float xlim_upper = 0.305;
   float ylim_lower = -0.1475;
   float ylim_upper = 0.1475;
-  float zlim_lower = 0.210;
+  float zlim_lower = 0.141087;
-  float zlim_upper = 0.218;
+  float zlim_upper = zlim_lower + 0.01;
 
-  //bool moved = false;
-  //
-  // TODO get pilz working
-  //std::unique_ptr<pilz_industrial_motion_planner::CommandListManager> command_list_manager_;
-  // command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
-
-  /**
-   * CommandListManager, used to plan MotionSequenceRequest
-   */
-  //pilz_industrial_motion_planner::CommandListManager command_list_manager;
-  //pilz_industrial_motion_planner::CommandListManager command_list_manager(*std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
 
   /**
    * Constructor
@@ -104,37 +90,41 @@ public:
   Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
     : RobotController(options),
       move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
-      //moveit_cpp_(std::shared_ptr<rclcpp::Node>(std::move(this))),
-      //planning_component_(MOVE_GROUP, moveit_cpp_),
-      //command_list_manager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel())
   {
+    // Declare parameters
+    this->declare_parameter("lite6_x_limit_lower", 0.1);
+    this->declare_parameter("lite6_x_limit_upper", 0.305);
+    this->declare_parameter("lite6_y_limit_lower", -0.1475);
+    this->declare_parameter("lite6_y_limit_upper", 0.1475);
+    this->declare_parameter("lite6_z_offset", 0.141087);
+    this->declare_parameter("lite6_z_lift_amount", 0.01);
+    this->declare_parameter("lite6_max_velocity_scaling_factor", 1.0);
+    this->declare_parameter("lite6_max_acceleration_scaling_factor", 0.6);
+    this->declare_parameter("lite6_blend_radius", 0.0);
+
+    xlim_lower = this->get_parameter("lite6_x_limit_lower").as_double();
+    xlim_upper = this->get_parameter("lite6_x_limit_upper").as_double();
+    ylim_lower = this->get_parameter("lite6_y_limit_lower").as_double();
+    ylim_upper = this->get_parameter("lite6_y_limit_upper").as_double();
+    zlim_lower = this->get_parameter("lite6_z_offset").as_double();
+    zlim_upper = zlim_lower + this->get_parameter("lite6_z_lift_amount").as_double();
 
-    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
-    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
-    // Use upper joint velocity and acceleration limits
     this->move_group.setMaxAccelerationScalingFactor(1.0);
+    this->move_group.setMaxVelocityScalingFactor(1.0);
+    //this->move_group.setMaxVelocityScalingFactor(0.8);
 
-    //setting this lower seems to improve overall time and prevents robot from moving too fast
+    trajectory_timer_ = this->create_wall_timer(
-    //this->move_group.setMaxVelocityScalingFactor(1.0);
+      10ms, std::bind(&Lite6Controller::executeTrajectoryFromQueue, this));
-    this->move_group.setMaxVelocityScalingFactor(0.8);
-
-    // Subscribe to target pose
-    //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
 
   }
 
   void setup()
   {
-    //moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(move_group.getNodeHandle());
     try
     {
-      //moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(this->shared_from_this());
-      //moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(move_group.getNodeHandle());
-      //planning_component_ = std::make_shared<moveit_cpp::PlanningComponent>(MOVE_GROUP, moveit_cpp_);
-      //
-
       planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(this->shared_from_this(),
                                                                                                "robot_description");
+
       this->sequence_client_ =
         this->create_client<moveit_msgs::srv::GetMotionSequence>("plan_sequence_path");
 
@@ -152,7 +142,6 @@ public:
       return;
     }
 
-
     move_group.setPlanningTime(30.0);
     RCLCPP_INFO(this->get_logger(), "Initialization successful.");
 
@@ -205,11 +194,12 @@ public:
   /**
    *
    */
-  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path, std::string planner_id)
   {
     moveit_msgs::msg::MotionSequenceRequest msr = moveit_msgs::msg::MotionSequenceRequest();
     //waypoints.push_back(move_group.getCurrentPose().pose);
-    std::string ee_link = move_group.getLinkNames().back();
+    //std::string ee_link = move_group.getLinkNames().back();
+    std::string ee_link = "pen_link";
     RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str());
     geometry_msgs::msg::Point previous_point;
     //previous_point.point.x = -1.0;
@@ -221,13 +211,17 @@ public:
       moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
 
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
-      mpr.planner_id = "PTP";
+      mpr.planner_id = planner_id;
+      //mpr.planner_id = "PTP";
       //mpr.planner_id = "LIN";
       mpr.group_name = move_group.getName();
-      mpr.max_velocity_scaling_factor = 1.0;
+      //mpr.max_velocity_scaling_factor = 1.0;
-      mpr.max_acceleration_scaling_factor = 0.98;
+      mpr.max_velocity_scaling_factor = this->get_parameter("lite6_max_velocity_scaling_factor").as_double();
-      mpr.allowed_planning_time = 10;
+      mpr.max_acceleration_scaling_factor = this->get_parameter("lite6_max_acceleration_scaling_factor").as_double();
-      mpr.max_cartesian_speed = 2; // m/s
+
+      //mpr.max_acceleration_scaling_factor = 0.1;
+      mpr.allowed_planning_time = 20;
+      //mpr.max_cartesian_speed = 2; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
 
       // A tolerance of 0.01 m is specified in position
@@ -256,9 +250,7 @@ public:
       mpr.goal_constraints.push_back(pose_goal);
 
       msi.req = mpr;
-      //msi.blend_radius = 6e-7; //TODO make configurable
+      msi.blend_radius = this->get_parameter("lite6_blend_radius").as_double();
-      //msi.blend_radius = 0.000001; //TODO make configurable
-      msi.blend_radius = 0.0; //TODO make configurable
       if (coincidentPoints(&pose.pose.position, &previous_point, msi.blend_radius + 1e-5))
       {
         RCLCPP_ERROR(this->get_logger(), "Detected coincident points, setting blend radius to 0.0");
@@ -274,13 +266,23 @@ public:
       msr.items.push_back(msi);
     }
     msr.items.back().blend_radius = 0.0; // Last element blend must be 0
-    moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState();
+
     moveit_msgs::msg::RobotState state_msg;
+    if (move_group_state == NULL)
+    {
+      RCLCPP_INFO(this->get_logger(), "Getting robot state");
+      move_group_state = move_group.getCurrentState(10);
+    }
     moveit::core::robotStateToRobotStateMsg(*move_group_state, state_msg);
     msr.items.front().req.start_state = state_msg;
     return msr;
   }
 
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  {
+    return createMotionSequenceRequest(path, "LIN");
+  }
+
   /**
    *
    */
@@ -382,6 +384,123 @@ public:
     }
   }
 
+  void executeTrajectoryFromQueue()
+  {
+    if (trajectory_queue.empty())
+    {
+      if (busy)
+        return;
+
+      busy = true;
+      //RCLCPP_INFO(this->get_logger(), "Getting robot state");
+      //move_group_state = move_group.getCurrentState(10);
+      busy = false;
+      return;
+    }
+    busy = true;
+    RCLCPP_INFO(this->get_logger(), "Executing next trajectory from queue");
+    move_group.execute(trajectory_queue.front());
+    trajectory_queue.pop();
+    busy = false;
+  }
+
+  moveit_msgs::msg::RobotTrajectory sendRequest(moveit_msgs::msg::MotionSequenceRequest msr)
+  {
+    auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
+    req->request = msr;
+    RCLCPP_INFO(this->get_logger(), "Sending request to sequence service");
+    auto res = sequence_client_->async_send_request(req);
+
+    auto ts = res.get()->response.planned_trajectories;
+
+    // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
+
+    if (ts.empty())
+    {
+      moveit_msgs::msg::RobotTrajectory t;
+      return t;
+    }
+
+
+    return ts[0];
+  }
+
+  void setMoveGroupState(moveit_msgs::msg::RobotTrajectory t)
+  {
+
+    if (t.joint_trajectory.points.empty())
+    {
+      RCLCPP_ERROR(this->get_logger(), "TRAJECTORY IS EMPTY");
+      return;
+    }
+
+    robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
+    rt.setRobotTrajectoryMsg(*move_group_state, t);
+    move_group_state = rt.getLastWayPointPtr();
+  }
+
+  void pathToTrajectory(const std::vector<geometry_msgs::msg::PoseStamped> *path, std::vector<moveit_msgs::msg::RobotTrajectory> *ts)
+  {
+    std::vector<geometry_msgs::msg::PoseStamped> penup = {};
+    std::vector<geometry_msgs::msg::PoseStamped> tail = {};
+    bool up = true;
+    if (!path)
+    {
+      RCLCPP_ERROR(this->get_logger(), "Received null pointer for path");
+      return;
+    }
+    for (auto p : *path)
+    {
+      if (p.pose.position.z > 0)
+        up = false;
+      if (up)
+        //penup->push_back(p);
+        penup.push_back(p);
+      else
+      {
+        up = false;
+        //tail->push_back(p);
+        tail.push_back(p);
+      }
+    }
+
+    if (!penup.empty())
+    {
+      auto msr = createMotionSequenceRequest(&penup, "PTP");
+      RCLCPP_ERROR(this->get_logger(), "Created MSR for penup");
+      if (msr.items.empty())
+      {
+        RCLCPP_ERROR(this->get_logger(), "MSR EMPTY");
+        return;
+      }
+      ts->push_back(sendRequest(msr));
+      //planAndLogOffset(&penup);
+      setMoveGroupState(ts->back());
+
+    }
+    else
+    {
+      RCLCPP_ERROR(this->get_logger(), "Penup path is empty, all motions will be LIN");
+    }
+    if (!tail.empty())
+    {
+      auto msr = createMotionSequenceRequest(&tail, "LIN");
+      RCLCPP_ERROR(this->get_logger(), "Created MSR for tail");
+      if (msr.items.empty())
+      {
+        RCLCPP_ERROR(this->get_logger(), "MSR EMPTY");
+        return;
+      }
+      ts->push_back(sendRequest(msr));
+      //planAndLogOffset(&tail);
+      setMoveGroupState(ts->back());
+    }
+    else
+    {
+      RCLCPP_ERROR(this->get_logger(), "Path was empty, no trajectories created");
+    }
+  }
+
   /**
    * Callback that executes path on robot
    */
@@ -394,31 +513,33 @@ public:
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
 
-    auto msr = createMotionSequenceRequest(&goal->motion.path);
+    std::vector<moveit_msgs::msg::RobotTrajectory> ts;
-    RCLCPP_INFO(this->get_logger(), "Created MSR");
+    pathToTrajectory(&goal->motion.path, &ts);
 
-    //planAndLogOffset(&goal->motion.path);
+    long unsigned int n = 0;
+
+    RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
+    for (auto t : ts)
+    {
+      RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
+      if (t.joint_trajectory.points.empty())
+      {
+        RCLCPP_ERROR(this->get_logger(), "TRAJECTORY IS EMPTY");
+        continue;
+      }
+      trajectory_queue.push(t);
+      n++;
+    }
 
-    RCLCPP_INFO(this->get_logger(), "Creating req");
-    auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
-    RCLCPP_INFO(this->get_logger(), "Setting msr request");
-    req->request = msr;
-    RCLCPP_INFO(this->get_logger(), "Sending request to sequence service");
-    auto res = sequence_client_->async_send_request(req);
-    RCLCPP_INFO(this->get_logger(), "Waiting for result");
-    auto ts = res.get()->response.planned_trajectories;
     std::string status = "";
-    if (ts.size() > 0)
+    if (n == ts.size())
     {
       status = "success";
-      RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
-      RCLCPP_INFO(this->get_logger(), "Executing result");
-      move_group.execute(ts[0]);
-
       status = status + "," + pointsToString(&goal->motion.path,0,0,0);
       appendLineToFile("OUTPUT.csv", status);
 
       result->result = "success";
+
       goal_handle->succeed(result);
       RCLCPP_INFO(this->get_logger(), "Goal succeeded");
       return;

src/lite6_controller/src/lite6_trajectory_executor.cpp

@@ -0,0 +1,111 @@
+#include <cstdio>
+#include <rclcpp/rclcpp.hpp>
+#include <robot_controller/robot_controller.hpp>
+#include <chrono>
+#include <cstdlib>
+#include <queue>
+
+#include <fstream>
+#include <iostream>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <moveit_msgs/msg/collision_object.hpp>
+
+//#include <moveit/planning_interface/planning_interface.h>
+//#include <moveit/planning_interface/planning_response.h>
+//#include <moveit/kinematic_constraints/kinematic_constraint.h>
+#include <moveit/kinematic_constraints/utils.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/common_planning_interface_objects/common_objects.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+
+
+#include <moveit_msgs/msg/constraints.hpp>
+#include <moveit_msgs/msg/robot_trajectory.hpp>
+#include <moveit_msgs/msg/motion_plan_request.hpp>
+#include <moveit_msgs/msg/motion_sequence_request.hpp>
+#include <moveit_msgs/msg/motion_sequence_item.hpp>
+#include <moveit_msgs/srv/get_motion_sequence.hpp>
+
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+
+#include <moveit/robot_trajectory/robot_trajectory.h>
+
+#include <pilz_industrial_motion_planner/command_list_manager.h>
+
+const std::string MOVE_GROUP = "lite6";
+
+using namespace std::chrono_literals;
+
+class TrajectoryExecutor : public rclcpp::Node
+{
+  public:
+    //trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 10);
+    rclcpp::Subscription<moveit_msgs::msg::RobotTrajectory>::SharedPtr subscription;
+    moveit::planning_interface::MoveGroupInterface move_group;
+
+    rclcpp::TimerBase::SharedPtr trajectory_timer_;
+    std::queue<moveit_msgs::msg::RobotTrajectory> trajectory_queue;
+    bool busy = false;
+
+    TrajectoryExecutor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
+      : Node("lite6_trajectory_executor",options),
+        move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+    {
+      subscription = this->create_subscription<moveit_msgs::msg::RobotTrajectory>(
+        "lite6_trajectory", 100, std::bind(&TrajectoryExecutor::trajectory_callback, this, std::placeholders::_1));
+
+      trajectory_timer_ = this->create_wall_timer(
+        10ms, std::bind(&TrajectoryExecutor::executeTrajectoryFromQueue, this));
+    }
+    void trajectory_callback(const moveit_msgs::msg::RobotTrajectory msg)
+    {
+      RCLCPP_INFO(this->get_logger(), "Received trajectory, adding to queue");
+      //move_group.execute(msg);
+      trajectory_queue.push(msg);
+    }
+
+    void executeTrajectoryFromQueue()
+    {
+      if (busy || trajectory_queue.empty())
+        return;
+      busy = true;
+      RCLCPP_INFO(this->get_logger(), "Executing next trajectory from queue");
+      move_group.execute(trajectory_queue.front());
+      trajectory_queue.pop();
+      busy = false;
+      RCLCPP_INFO(this->get_logger(), "Finished executing trajectory");
+    }
+};
+
+/**
+ * Starts lite6_controller
+ */
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  auto trajectory_executor = std::make_shared<TrajectoryExecutor>();
+
+  // TODO remove sleep if not necessary
+  // Sleep in case move_group not loaded
+  rclcpp::sleep_for(2s);
+
+  //rclcpp::executors::SingleThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
+  //executor.add_node(lite6);
+  executor.add_node(trajectory_executor);
+  executor.spin();
+
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

src/robot_controller/src/cpp/dummy_controller.cpp

@@ -23,7 +23,7 @@ class DummyController : public RobotController
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
     RCLCPP_INFO(this->get_logger(), "Executing goal");
-    rclcpp::Rate loop_rate(20);
+    rclcpp::Rate loop_rate(100);
     const auto goal = goal_handle->get_goal();
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();

src/virtual_drawing_surface/CMakeLists.txt

@@ -40,6 +40,6 @@ endif()
 
 # Install directories
 #install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME})
-install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+#install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 
 ament_package()

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 = 3 # 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.2485, 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")