Move execution to new ROS2 node

2023-03-27 11:44:25 +03:00
27 changed files with 242 additions and 766 deletions
--- a/7
+++ b/7
@@ -12,8 +12,6 @@ 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}
@@ -27,9 +25,6 @@ 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
@@ -80,7 +75,7 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
    rm -rf ${WS_LOG_DIR}

 # Copy example svg images
-COPY ./svg test-images
+COPY ./svg svg

 ### Add workspace to the ROS entrypoint
 ### Source ROS workspace inside `~/.bashrc` to enable autocompletion
--- a/README.md
+++ b/README.md
@@ -35,23 +35,6 @@ 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:
@@ -77,42 +60,26 @@ DummyController echoes Motion messages to the terminal.
 ros2 run robot_controller dummy_controller
 ```

-### 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.
+AxidrawController draws on the axidraw robot
 ``` sh
-ros2 launch axidraw_controller axidraw_controller.launch.py serial_port:=/dev/ttyACM0 config:=./config.yaml
+ros2 launch axidraw_controller axidraw_controller
 ```

-### Lite6Controller
 This starts the simulated lite6
 ``` sh
-ros2 launch lite6_controller lite6_gazebo.launch.py config:=./config.yaml
+ros2 launch lite6_controller lite6_gazebo.launch.py
 ```

 This runs the real lite6
 ``` sh
-ros2 launch lite6_controller lite6_real.launch.py config:=./config.yaml
+ros2 launch lite6_controller lite6_real.launch.py
 ```

 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 config:=./config.yaml
+ros2 launch lite6_controller lite6_real_no_gui.launch.py
 ```

-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
@@ -120,23 +87,6 @@ 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
@@ -227,7 +177,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             |             |

--- a/config.yaml
+++ b/config.yaml
@@ -1,26 +0,0 @@
-/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
--- a/scripts/log_drawing.sh
+++ b/scripts/log_drawing.sh
@@ -1,12 +0,0 @@
-#!/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'}
--- a/src/axidraw_controller/CMakeLists.txt
+++ b/src/axidraw_controller/CMakeLists.txt
@@ -40,11 +40,6 @@ 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
--- a/src/axidraw_controller/config/config.yaml
+++ b/src/axidraw_controller/config/config.yaml
@@ -1,12 +0,0 @@
-/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
--- a/src/axidraw_controller/launch/axidraw_controller.launch.py
+++ b/src/axidraw_controller/launch/axidraw_controller.launch.py
@@ -1,5 +1,4 @@
 import os
-import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -19,13 +18,6 @@ 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",
@@ -42,7 +34,6 @@ def launch_setup(context, *args, **kwargs):
            arguments=["--ros-args", "--log-level", log_level],
            parameters=[
                {"serial_port": serial_port},
-                axidraw_config,
            ],
        ),
    ]
--- a/src/axidraw_controller/src/cpp/axidraw_controller.cpp
+++ b/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++;
--- a/src/axidraw_controller/src/py/axidraw_serial.py
+++ b/src/axidraw_controller/src/py/axidraw_serial.py
@@ -42,7 +42,6 @@ class AxidrawSerial(Node):
    status = {
        "serial": "not ready",
        "motion": "waiting",
-        "pen": "up",
    }

    def init_serial(self, port):
@@ -61,22 +60,9 @@ 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):
@@ -94,22 +80,10 @@ 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:{}, retrying".format(port))
-        self.get_logger().info("Successfully connected to axidraw on port:{}".format(port))
+            self.get_logger().error("Failed to connect 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))
@@ -191,9 +165,7 @@ 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):
@@ -206,9 +178,7 @@ 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):
@@ -223,7 +193,6 @@ 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()


--- a/src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml
+++ b/src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml
@@ -1,5 +0,0 @@
-cartesian_limits:
-  max_trans_vel: 0.4
-  max_trans_acc: 4.00
-  max_trans_dec: -5.0
-  max_rot_vel: 1.57
--- a/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml
+++ b/src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml
@@ -4,31 +4,31 @@
 joint_limits:
  joint1:
    has_velocity_limits: true
-    max_velocity: 4.14
+    max_velocity: 2.14
    has_acceleration_limits: true
-    max_acceleration: 30.0
+    max_acceleration: 1.0
  joint2:
    has_velocity_limits: true
-    max_velocity: 4.14
+    max_velocity: 2.14
    has_acceleration_limits: true
-    max_acceleration: 30.0
+    max_acceleration: 1.0
  joint3:
    has_velocity_limits: true
-    max_velocity: 4.14
+    max_velocity: 2.14
    has_acceleration_limits: true
-    max_acceleration: 30.0
+    max_acceleration: 1.0
  joint4:
    has_velocity_limits: true
-    max_velocity: 4.14
+    max_velocity: 2.14
    has_acceleration_limits: true
-    max_acceleration: 30.0
+    max_acceleration: 1.0
  joint5:
    has_velocity_limits: true
-    max_velocity: 4.14
+    max_velocity: 2.14
    has_acceleration_limits: true
-    max_acceleration: 30.0
+    max_acceleration: 1.0
  joint6:
    has_velocity_limits: true
-    max_velocity: 4.14
+    max_velocity: 2.14
    has_acceleration_limits: true
-    max_acceleration: 30.0
+    max_acceleration: 1.0
--- a/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
+++ b/src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
@@ -65,22 +65,19 @@ def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None,


 def get_xarm_robot_description_parameters(
-    xacro_urdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-    xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+    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={},
    srdf_arguments={},
    arguments={}):
    urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware')
-    moveit_config_package_name = 'custom_xarm_moveit_config'
+    moveit_config_package_name = '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('custom_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')
    
-    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, 
@@ -90,6 +87,6 @@ def get_xarm_robot_description_parameters(
            xacro_file=xacro_srdf_file,
            arguments=srdf_arguments
        ),
-        'robot_description_planning': joint_limits,
+        'robot_description_planning': load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml'),
        'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
    }
--- a/src/drawing_controller/drawing_controller/drawing_controller.py
+++ b/src/drawing_controller/drawing_controller/drawing_controller.py
@@ -17,8 +17,6 @@ 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):
@@ -63,17 +61,13 @@ class DrawingController(Node):
    def __init__(self, svgpath):
        super().__init__('drawing_controller')
        #self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10)
-        timer_period = 0.1  # seconds
+        timer_period = 1.0  # 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)
@@ -91,11 +85,7 @@ class DrawingController(Node):

        self.svg_processor = SVGProcessor(self.get_logger())
        self.svg = self.svg_processor.process_svg(svgpath)
-        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')
+        self.get_logger().info('Ready to begin executing motions')

    def send_goal(self, motion):
        self.busy = True
@@ -111,23 +101,22 @@ class DrawingController(Node):
    def goal_response_callback(self, future):
        goal_handle = future.result()
        if not goal_handle.accepted:
-            self.get_logger().debug('Goal rejected :(')
+            self.get_logger().info('Goal rejected :(')
            return

-        self.get_logger().debug('Goal accepted :)')
+        self.get_logger().info('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.results.update({ result.result: self.results.get(result.result, 0) + 1 })
-        self.get_logger().info('Result: {0}'.format(result.result))
+        self.get_logger().info('Result: {0}'.format(result))
        self.busy = False

    def feedback_callback(self, feedback_msg):
        feedback = feedback_msg.feedback
-        self.get_logger().debug('Received feedback: {0}'.format(feedback))
+        self.get_logger().info('Received feedback: {0}'.format(feedback))

    def append_points(self, motion, points):
        for point in points:
@@ -146,23 +135,17 @@ 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(self.i, len(self.svg)))
+        self.get_logger().info('Executing motion: {}...'.format(motion.path[:10]))
        self.send_goal(motion)


--- a/src/drawing_controller/drawing_controller/svg_path_parser.py
+++ b/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.debug("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr)))
+        self.logger.info("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.debug("Parsing path :'{}...' with {} tokens".format(path[:20], len(path)))
+        self.logger.info("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
-            n = 100
-            curve = cf.bezier(control_points)
+            control_points = control_points / maxval #normalize values
+            n = 50
+            curve = cf.cubic_curve(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,20 +136,6 @@ 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]
@@ -222,6 +208,7 @@ 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()),
@@ -234,6 +221,7 @@ 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()),
@@ -250,27 +238,9 @@ class SVGPathParser():
                self.logger.error("SVG path parser '{}' not implemented".format(w))
            # Quadratic Bézier Curve commands
            if (w == "Q"):
-                while True:
-                    control_points = [(x,y),
-                                      (getnum(),getnum()),
-                                      (getnum(),getnum())]
-                    coordinates = quadratic_bezier(control_points)
-                    appendpoints(coordinates)
-                    if not nextisnum():
-                        break
-                i += 1
-                continue
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
            if (w == "q"):
-                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
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
            if (w == "T"):
                self.logger.error("SVG path parser '{}' not implemented".format(w))
            if (w == "t"):
@@ -289,5 +259,5 @@ class SVGPathParser():
            self.logger.error("SVG path parser panic mode at '{}'".format(w))

            i += 1
-        self.logger.debug("Finished parsing path :'{}...' with {} points".format(output[:3], len(output)))
+        self.logger.info("Finished parsing path :'{}...' with {} points".format(output[:3], len(output)))
        return output
--- a/src/drawing_controller/drawing_controller/svg_processor.py
+++ b/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.0001
+        tolerance = 0.001
        prev = (-1, -1, 0)
        for i, p in enumerate(motion):
            x = p[0]
@@ -197,7 +197,8 @@ 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.00005
+        #epsilon = 0.009
+        epsilon = 0.005

        tmp = []
        out = []
@@ -214,7 +215,6 @@ class SVGProcessor():
                tmp.append(list(p)[:-1])
            lastup = penup

-        # Handle anything left in tmp
        if (len(tmp) > 0):
            out += sf(tmp)

--- a/src/lite6_controller/CMakeLists.txt
+++ b/src/lite6_controller/CMakeLists.txt
@@ -16,8 +16,6 @@ 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)
@@ -46,11 +44,8 @@ ament_target_dependencies(lite6_controller
  "robot_controller"
  "moveit_ros_planning_interface"
  "robot_interfaces")
-
-add_executable(lite6_calibration src/lite6_calibration.cpp)
-ament_target_dependencies(lite6_calibration
-  "xarm_msgs"
-  "xarm_api"
+add_executable(lite6_trajectory_executor src/lite6_trajectory_executor.cpp)
+ament_target_dependencies(lite6_trajectory_executor
  "rclcpp"
  "moveit"
  "rclcpp_action"
@@ -67,14 +62,9 @@ endif()

 install(TARGETS
  lite6_controller
-  lite6_calibration
+  lite6_trajectory_executor
  DESTINATION lib/${PROJECT_NAME})

 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

-install(DIRECTORY
-  config
-  DESTINATION share/${PROJECT_NAME}
-)
-
 ament_package()
--- a/src/lite6_controller/config/config.yaml
+++ b/src/lite6_controller/config/config.yaml
@@ -1,13 +0,0 @@
-/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
--- a/src/lite6_controller/launch/lite6_gazebo.launch.py
+++ b/src/lite6_controller/launch/lite6_gazebo.launch.py
@@ -2,7 +2,6 @@
 """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
@@ -18,8 +17,7 @@ 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("custom_xarm_description"), "rviz", "display.rviz"))
-
+    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')
@@ -56,8 +54,6 @@ 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(
@@ -195,6 +191,17 @@ def launch_setup(context, *args, **kwargs):
                {"use_sim_time": use_sim_time},
            ],
        ),
+        Node(
+            package="lite6_controller",
+            executable="lite6_trajectory_executor",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                robot_description,
+                robot_description_semantic,
+                {"use_sim_time": use_sim_time},
+            ],
+        ),
        Node(
            package="virtual_drawing_surface",
            executable="drawing_surface.py",
@@ -267,20 +274,10 @@ 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):
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    for i in range(1,7):
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
+        joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0

    kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
    #kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin'
@@ -314,8 +311,12 @@ def launch_setup(context, *args, **kwargs):
        prefix=prefix.perform(context))


-    #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']
+    robot_description_parameters['cartesian_limits'] = {}
+    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57
+

    # Planning pipeline
    # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py
@@ -392,8 +393,15 @@ def launch_setup(context, *args, **kwargs):
        'publish_geometry_updates': True,
        'publish_state_updates': True,
        'publish_transforms_updates': True,
-        'publish_robot_description': True,
-        'publish_robot_description_semantic' :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
--- a/src/lite6_controller/launch/lite6_real.launch.py
+++ b/src/lite6_controller/launch/lite6_real.launch.py
@@ -1,5 +1,4 @@
 import os
-import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -16,7 +15,6 @@ 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='')
@@ -57,8 +55,6 @@ 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(
@@ -79,7 +75,7 @@ def launch_setup(context, *args, **kwargs):
    # 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'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
        launch_arguments={
            'prefix': prefix,
            'hw_ns': hw_ns,
@@ -109,12 +105,12 @@ def launch_setup(context, *args, **kwargs):

    # robot_description_parameters
    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'custom_xarm_moveit_config'
+    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('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        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('/'),
@@ -158,22 +154,21 @@ 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']

-    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
+    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

+    #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')
@@ -191,11 +186,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):
-    #    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'] = 2.5
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 3.0
+    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.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

    add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
    add_prefix_to_moveit_params(
@@ -237,8 +232,15 @@ def launch_setup(context, *args, **kwargs):
        'publish_geometry_updates': True,
        'publish_state_updates': True,
        'publish_transforms_updates': True,
-        'publish_robot_description': True,
-        'publish_robot_description_semantic' :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,
+        # },
    }


@@ -247,8 +249,7 @@ 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": "LIN",
+            "default_planner_config": "PTP",
        }
    }

@@ -263,7 +264,6 @@ 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,
--- a/src/lite6_controller/launch/lite6_real_no_gui.launch.py
+++ b/src/lite6_controller/launch/lite6_real_no_gui.launch.py
@@ -1,5 +1,4 @@
 import os
-import yaml
 from launch import LaunchDescription
 from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -16,7 +15,6 @@ 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='')
@@ -57,8 +55,6 @@ 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(
@@ -79,7 +75,7 @@ def launch_setup(context, *args, **kwargs):
    # 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'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.launch.py'])),
        launch_arguments={
            'prefix': prefix,
            'hw_ns': hw_ns,
@@ -109,12 +105,12 @@ def launch_setup(context, *args, **kwargs):

    # robot_description_parameters
    # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
-    moveit_config_package_name = 'custom_xarm_moveit_config'
+    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('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        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('/'),
@@ -159,16 +155,10 @@ 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):
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
-    #    joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
+    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

    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)))
@@ -192,8 +182,11 @@ def launch_setup(context, *args, **kwargs):
        kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
        prefix=prefix.perform(context))

-    #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']
+    robot_description_parameters['cartesian_limits'] = {}
+    robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
+    robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
+    robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
+    robot_description_parameters['cartesian_limits']['max_rot_vel'] =  1.57

    # Planning Configuration
    ompl_planning_pipeline_config = {
@@ -243,8 +236,15 @@ def launch_setup(context, *args, **kwargs):
        'publish_geometry_updates': True,
        'publish_state_updates': True,
        'publish_transforms_updates': True,
-        'publish_robot_description': True,
-        'publish_robot_description_semantic' :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
--- a/src/lite6_controller/package.xml
+++ b/src/lite6_controller/package.xml
@@ -15,8 +15,6 @@
  <!-- <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>
--- a/src/lite6_controller/src/lite6_calibration.cpp
+++ b/src/lite6_controller/src/lite6_calibration.cpp
@@ -1,131 +0,0 @@
-#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;
-}
--- a/src/lite6_controller/src/lite6_controller.cpp
+++ b/src/lite6_controller/src/lite6_controller.cpp
@@ -47,6 +47,15 @@ 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
 */
@@ -63,6 +72,8 @@ public:
  rclcpp::TimerBase::SharedPtr trajectory_timer_;
  bool busy = false;

+  rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr trajectory_pub;
+
  /**
   * TODO Use instead of MoveGroupInterface
   * https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
@@ -80,9 +91,20 @@ public:
  float xlim_upper = 0.305;
  float ylim_lower = -0.1475;
  float ylim_upper = 0.1475;
-  float zlim_lower = 0.141087;
-  float zlim_upper = zlim_lower + 0.01;
+  float zlim_lower = 0.210;
+  float zlim_upper = 0.218;

+  //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
@@ -90,29 +112,27 @@ 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
+    //this->move_group.setMaxVelocityScalingFactor(1.0);
+    this->move_group.setMaxVelocityScalingFactor(0.8);
+
+
+    trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 100);
+
+    // 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));
+
+    //
    trajectory_timer_ = this->create_wall_timer(
      10ms, std::bind(&Lite6Controller::executeTrajectoryFromQueue, this));

@@ -120,10 +140,18 @@ public:

  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");
+      //RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Getting initial robot state");
+      //move_group.startStateMonitor(5);

      this->sequence_client_ =
        this->create_client<moveit_msgs::srv::GetMotionSequence>("plan_sequence_path");
@@ -194,12 +222,11 @@ public:
  /**
   *
   */
-  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path, std::string planner_id)
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
  {
    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 = "pen_link";
+    std::string ee_link = move_group.getLinkNames().back();
    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;
@@ -211,17 +238,13 @@ public:
      moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();

      planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
-      mpr.planner_id = planner_id;
-      //mpr.planner_id = "PTP";
+      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 = this->get_parameter("lite6_max_velocity_scaling_factor").as_double();
-      mpr.max_acceleration_scaling_factor = this->get_parameter("lite6_max_acceleration_scaling_factor").as_double();
-
-      //mpr.max_acceleration_scaling_factor = 0.1;
-      mpr.allowed_planning_time = 20;
-      //mpr.max_cartesian_speed = 2; // m/s
+      mpr.max_velocity_scaling_factor = 1.0;
+      mpr.max_acceleration_scaling_factor = 0.98;
+      mpr.allowed_planning_time = 10;
+      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
@@ -250,7 +273,9 @@ public:
      mpr.goal_constraints.push_back(pose_goal);

      msi.req = mpr;
-      msi.blend_radius = this->get_parameter("lite6_blend_radius").as_double();
+      //msi.blend_radius = 6e-7; //TODO make configurable
+      //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");
@@ -266,7 +291,6 @@ public:
      msr.items.push_back(msi);
    }
    msr.items.back().blend_radius = 0.0; // Last element blend must be 0
-
    moveit_msgs::msg::RobotState state_msg;
    if (move_group_state == NULL)
    {
@@ -278,11 +302,6 @@ public:
    return msr;
  }

-  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
-  {
-    return createMotionSequenceRequest(path, "LIN");
-  }
-
  /**
   *
   */
@@ -386,17 +405,8 @@ public:

  void executeTrajectoryFromQueue()
  {
-    if (trajectory_queue.empty())
-    {
-      if (busy)
+    if (busy || trajectory_queue.empty())
      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());
@@ -404,103 +414,6 @@ public:
    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
   */
@@ -513,33 +426,38 @@ public:
    auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
    auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();

-    std::vector<moveit_msgs::msg::RobotTrajectory> ts;
-    pathToTrajectory(&goal->motion.path, &ts);
+    auto msr = createMotionSequenceRequest(&goal->motion.path);
+    RCLCPP_INFO(this->get_logger(), "Created MSR");

-    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++;
-    }
+    //planAndLogOffset(&goal->motion.path);

+    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 (n == ts.size())
+    if (ts.size() > 0)
    {
      status = "success";
+      RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
+      RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
+
+      //trajectory_queue.push(ts[0]);
+      this->trajectory_pub->publish(ts[0]);
+
+      // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
+      robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
+      rt.setRobotTrajectoryMsg(*move_group_state, ts[0]);
+      move_group_state = rt.getLastWayPointPtr();
+
      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;
@@ -559,6 +477,27 @@ public:
  }
 };

+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;
+    TrajectoryExecutor()
+      : Node("lite6_trajectory_executor"),
+        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));
+    }
+    void trajectory_callback(const moveit_msgs::msg::RobotTrajectory msg)
+    {
+      RCLCPP_INFO(this->get_logger(), "Received trajectory, executing");
+      move_group.execute(msg);
+      RCLCPP_INFO(this->get_logger(), "Finished executing trajectory");
+    }
+};
+
 /**
 * Starts lite6_controller
 */
@@ -579,6 +518,7 @@ int main(int argc, char ** argv)
  //rclcpp::executors::SingleThreadedExecutor executor;
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(lite6);
+  //executor.add_node(trajectory_executor);
  executor.spin();

  rclcpp::shutdown();
--- a/src/lite6_controller/src/lite6_trajectory_executor.cpp
+++ b/src/lite6_controller/src/lite6_trajectory_executor.cpp
@@ -1,111 +0,0 @@
-#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;
-}
--- a/src/robot_controller/src/cpp/dummy_controller.cpp
+++ b/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(100);
+    rclcpp::Rate loop_rate(20);
    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>();
--- a/src/virtual_drawing_surface/CMakeLists.txt
+++ b/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()
--- a/src/virtual_drawing_surface/src/py/drawing_surface.py
+++ b/src/virtual_drawing_surface/src/py/drawing_surface.py
@@ -73,7 +73,7 @@ class DrawingApp(tk.Tk):
    def draw(self,x,y,z):
        # putpixel is too slow
        #self.img.putpixel((int(x), int(y)), (255, 0, 0))
-        r = 3 # radius
+        r = 4 # radius
        for xp in range(max(0, x-r), min(self.width-1, x+r)):
            for yp in range(max(0, y-r), min(self.height-1, y+r)):
                self.arr[xp,yp,0] = 0 #red
@@ -105,7 +105,7 @@ class DrawingApp(tk.Tk):
            #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))
+            y = int(translate(p.x, -0.3485, 0.1, 0, self.height))

            #x = bound(self.width - x, 0, self.width)
            #y = bound(self.height - y, 0, self.height)