Add splipy and initial cubic curve path command

.docker/build.bash

@@ -3,7 +3,7 @@
 SCRIPT_DIR="$(cd "$(dirname "$(readlink -f "${BASH_SOURCE[0]}")")" &>/dev/null && pwd)"
 PROJECT_DIR="$(dirname "${SCRIPT_DIR}")"
 
-TAG="cacdar/$(basename "${PROJECT_DIR}")"
+TAG="cacdar/drawing-robot-ros2"
 
 if [ "${#}" -gt "0" ]; then
     if [[ "${1}" != "-"* ]]; then

.gitignore

@@ -5,3 +5,11 @@
 
 # Python
 **/__pycache__
+
+# Sphinx
+**/_build
+
+# Doxygen
+**/html
+**/latex
+**/xml

Dockerfile

@@ -34,6 +34,11 @@ RUN apt-get update && \
     pip install --upgrade --upgrade-strategy eager packaging && \
     pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
 
+### Install splipy
+RUN apt-get update && \
+    apt-get install -yq python3-pip && \
+    pip install --upgrade --upgrade-strategy eager splipy
+
 ### Import and install dependencies, then build these dependencies (not ign_moveit2_examples yet)
 COPY ./drawing_robot_ros2.repos ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos
 RUN vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/ign_moveit2_examples/drawing_robot_ros2.repos && \

Makefile

@@ -0,0 +1,20 @@
+# Minimal makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line, and also
+# from the environment for the first two.
+SPHINXOPTS    ?=
+SPHINXBUILD   ?= sphinx-build
+SOURCEDIR     = .
+BUILDDIR      = _build
+
+# Put it first so that "make" without argument is like "make help".
+help:
+	@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
+
+.PHONY: help Makefile
+
+# Catch-all target: route all unknown targets to Sphinx using the new
+# "make mode" option.  $(O) is meant as a shortcut for $(SPHINXOPTS).
+%: Makefile
+	@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)

conf.py

@@ -0,0 +1,33 @@
+# Configuration file for the Sphinx documentation builder.
+#
+# For the full list of built-in configuration values, see the documentation:
+# https://www.sphinx-doc.org/en/master/usage/configuration.html
+
+# -- Project information -----------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information
+
+project = 'drawing_robot_ros2'
+copyright = '2023, Nicolas Hiillos'
+author = 'Nicolas Hiillos'
+
+# -- General configuration ---------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration
+
+#extensions = ['sphinx.ext.pngmath', 'sphinx.ext.todo', 'breathe' ]
+extensions = [ 'breathe' ]
+
+templates_path = ['_templates']
+exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store']
+
+breathe_projects = {
+    "robot_controller": "src/robot_controller/xml",
+    "lite6_controller": "src/lite6_controller/xml",
+    "axidraw_controller": "src/axidraw_controller/xml",
+}
+
+
+# -- Options for HTML output -------------------------------------------------
+# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output
+
+html_theme = 'alabaster'
+html_static_path = ['_static']

gen-docs.sh

@@ -0,0 +1,15 @@
+#!/usr/bin/env sh
+
+pushd src/robot_controller/
+doxygen
+popd
+
+pushd src/lite6_controller/
+doxygen
+popd
+
+pushd src/axidraw_controller/
+doxygen
+popd
+
+make html

index.rst

@@ -0,0 +1,23 @@
+.. drawing_robot_ros2 documentation master file, created by
+   sphinx-quickstart on Fri Jan 20 12:29:28 2023.
+   You can adapt this file completely to your liking, but it should at least
+   contain the root `toctree` directive.
+
+Welcome to drawing_robot_ros2's documentation!
+==============================================
+
+.. toctree::
+   :maxdepth: 2
+   :caption: Contents:
+
+.. doxygenclass:: robot_controller
+   :project: robot_controller
+   :members:
+
+
+Indices and tables
+==================
+
+* :ref:`genindex`
+* :ref:`modindex`
+* :ref:`search`

make.bat

@@ -0,0 +1,35 @@
+@ECHO OFF
+
+pushd %~dp0
+
+REM Command file for Sphinx documentation
+
+if "%SPHINXBUILD%" == "" (
+	set SPHINXBUILD=sphinx-build
+)
+set SOURCEDIR=.
+set BUILDDIR=_build
+
+%SPHINXBUILD% >NUL 2>NUL
+if errorlevel 9009 (
+	echo.
+	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+	echo.installed, then set the SPHINXBUILD environment variable to point
+	echo.to the full path of the 'sphinx-build' executable. Alternatively you
+	echo.may add the Sphinx directory to PATH.
+	echo.
+	echo.If you don't have Sphinx installed, grab it from
+	echo.https://www.sphinx-doc.org/
+	exit /b 1
+)
+
+if "%1" == "" goto help
+
+%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+goto end
+
+:help
+%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+
+:end
+popd

rebuild.sh

@@ -13,7 +13,8 @@ rosdep install -y -r -i --rosdistro "humble" --from-paths src
 cd src
 rm -r install build log
 #colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
-colcon build --packages-select robot_interfaces robot_controller
+#colcon build --packages-select robot_interfaces robot_controller
+colcon build --paths robot_interfaces robot_controller
 source install/local_setup.bash
 colcon build
 source install/local_setup.bash

src/axidraw_controller/launch/axidraw_controller.launch.py

@@ -16,7 +16,7 @@ from launch.events import Shutdown
 
 def launch_setup(context, *args, **kwargs):
     serial_port = LaunchConfiguration('serial_port', default='/dev/ttyACM0')
-    log_level = LaunchConfiguration("log_level", default='warn')
+    log_level = LaunchConfiguration("log_level", default='info')
 
     nodes = [
         Node(

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -17,7 +17,10 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 
 using namespace std::chrono_literals;
-// Controller for the axidraw robot. Calls axidraw python API over ROS2 topics
+
+/**
+ * Controller for the axidraw robot. Calls axidraw python API over ROS2 topics
+ */
 class AxidrawController : public RobotController
 {
   rclcpp::Client<robot_interfaces::srv::Status>::SharedPtr status_client;
@@ -31,15 +34,16 @@ class AxidrawController : public RobotController
   {
     status_client = this->create_client<robot_interfaces::srv::Status>("axidraw_status");
 
-    while (!status_client->wait_for_service(1s))
+    while (!status_client->wait_for_service(5s))
     {
       if (!rclcpp::ok())
       {
         RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for axidraw_status");
         //return 0;
       }
-      RCLCPP_ERROR(this->get_logger(), "Failed to connect to axidraw_status");
+      RCLCPP_WARN(this->get_logger(), "Failed to connect to axidraw_status, retrying");
     }
+    RCLCPP_INFO(this->get_logger(), "Successfully connected to axidraw_status, Axidraw is ready");
 
     // Create publishers for axidraw_serial.py topics
     move_pub = this->create_publisher<geometry_msgs::msg::Point>("axidraw_move", 10);
@@ -48,7 +52,9 @@ class AxidrawController : public RobotController
     path_pub = this->create_publisher<robot_interfaces::msg::Points>("axidraw_path", 10);
   }
 
-  // Return true if axidraw is ready
+  /**
+   * Return true if axidraw is ready
+   */
   bool is_ready()
   {
     auto request = std::make_shared<robot_interfaces::srv::Status::Request>();
@@ -56,13 +62,18 @@ class AxidrawController : public RobotController
     auto result = status_client->async_send_request(request);
 
     result.wait();
-    return result.get()->status == "waiting";
+    std::string res_string = result.get()->status;
+    RCLCPP_INFO(this->get_logger(), "Called is_ready(), status: %s", res_string.c_str());
+    return res_string == "ready";
   }
 
   // Set limits for A4 paper: 297x210mm
   float xlim = 297;
   float ylim = 210;
 
+  /**
+   * Function that translates an input value with a given range to a value within another range.
+   */
   float translate(float val, float lmin, float lmax, float rmin, float rmax)
   {
     float lspan = lmax - lmin;
@@ -71,7 +82,9 @@ class AxidrawController : public RobotController
     return rmin + (val * rspan);
   }
 
-  // Translate a pose to axidraw coordinates (mm)
+  /**
+   * Translate a pose to axidraw coordinates (mm)
+   */
   geometry_msgs::msg::Point translate_pose(geometry_msgs::msg::PoseStamped ps)
   {
     auto pose = ps.pose;
@@ -96,7 +109,9 @@ class AxidrawController : public RobotController
   }
 
 
-  /// Callback that executes path on robot
+  /**
+   * Callback that executes path on robot
+   */
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
     RCLCPP_INFO(this->get_logger(), "Executing goal");
@@ -130,7 +145,11 @@ class AxidrawController : public RobotController
 
       // Ensure axidraw is not busy
       while (!is_ready())
+      {
+        feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " waiting for previous motion";
+        goal_handle->publish_feedback(feedback);
         loop_rate.sleep();
+      }
 
       if (p.z > 0)
         this->penup_pub->publish(std_msgs::msg::Empty());
@@ -139,7 +158,11 @@ class AxidrawController : public RobotController
 
       // Wait for pen movement to complete
       while (!is_ready())
+      {
+        feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " waiting for current motion";
+        goal_handle->publish_feedback(feedback);
         loop_rate.sleep();
+      }
 
       this->move_pub->publish(p);
 
@@ -161,6 +184,9 @@ class AxidrawController : public RobotController
   }
 };
 
+/**
+ *
+ */
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

src/axidraw_controller/src/py/axidraw_serial.py

@@ -10,32 +10,49 @@ import rclpy
 from rclpy.node import Node
 from rclpy.qos import QoSProfile
 
-
-#TODO delete this
-class Test():
-    def __init__():
-        ad = axidraw.AxiDraw()          # Initialize class
-        ad.interactive()                # Enter interactive context
-        ad.options.port = "/dev/ttyACM0"
-        if not ad.connect():            # Open serial port to AxiDraw;
-            quit()                      #   Exit, if no connection.
-        ad.options.units = 2            # set working units to mm.
-        ad.options.model = 2            # set model to AxiDraw V3/A3
-        # Absolute moves follow:
-        ad.moveto(10, 10)               # Pen-up move to (10mm, 10mm)
-        ad.lineto(20, 10)               # Pen-down move, to (20mm, 10)
-        ad.moveto(0, 0)                 # Pen-up move, back to origin.
-        ad.disconnect()                 # Close serial port to AxiDraw
-
-
 #TODO handle serial disconnect
+
 class AxidrawSerial(Node):
+    """
+    Class for forwarding axidraw python API functions to ROS2 topics.
+
+    ...
+
+    Attributes
+    ----------
+    status : dict
+        contains the robot's status
+
+    Methods
+    -------
+    init_serial(port)
+
+    Services
+    -------
+    Status, 'axidraw_status'
+
+    Topics
+    -------
+    Point, 'axidraw_move'
+    Empty, 'axidraw_penup'
+    Empty, 'axidraw_pendown'
+    Points, 'axidraw_path'
+    """
+
     status = {
         "serial": "not ready",
         "motion": "waiting",
     }
 
     def init_serial(self, port):
+        '''
+        Initiates connection to axidraw over serial.
+
+            Parameters:
+                    port (string): The serial port or path to serial device (example: "/dev/ttyACM0")
+            Returns:
+                    False if connection failed and True if it succeeded.
+        '''
         self.ad = axidraw.AxiDraw()          # Initialize class
         self.ad.interactive()                # Enter interactive context
         self.ad.options.port = port
@@ -49,6 +66,15 @@ class AxidrawSerial(Node):
         return True
 
     def __init__(self):
+        """
+        Sets up a connection to the axidraw robot and starts the 'axidraw_***' services and topic subscriptions.
+        Retries connection to axidraw if it fails.
+        Fetches port from ROS2 parameter 'serial_port', defaulting to '/dev/ttyACM0'.
+
+        Parameters
+        ----------
+        """
+
         super().__init__('axidraw_serial')
 
         self.declare_parameter('serial_port', '/dev/ttyACM0')
@@ -65,16 +91,63 @@ class AxidrawSerial(Node):
         self.path_sub = self.create_subscription(Points, 'axidraw_path', self.stroke_callback, qos_profile=QoSProfile(depth=1))
 
     def get_status(self, request, response):
-        response.status = status.get(request.resource, "Resource '{}' not found.".format(request.resource))
+        '''
+        Looks up the status of the requested resource, sets it as the response status and returns it.
+        Used directly by the "axidraw_status" service.
+
+            Parameters:
+                    request (robot_interfaces.srv.Status.request): The request
+                    response (robot_interfaces.srv.Status.response): The response
+            Returns:
+                    response (robot_interfaces.srv.Status.response): The response with the data added. If not found returns "Resource 'X' not found.".
+        '''
+        response.status = self.status.get(request.resource, "Resource '{}' not found.".format(request.resource))
         return response
 
+    def go_home(self):
+        '''
+        Moves the robot to (0,0).
+            Parameters:
+            Returns:
+        '''
+        self.status["motion"] = "busy"
+        if self.status["serial"] == "ready":
+            self.ad.moveto(0,0)
+        self.status["motion"] = "ready"
+
     def set_busy(self):
-        self["motion"] = "busy"
+        '''
+        Sets the robot motion to "busy"
+            Parameters:
+            Returns:
+        '''
+        self.status["motion"] = "busy"
 
     def set_ready(self):
-        self["motion"] = "ready"
+        '''
+        Sets the robot motion to "ready"
+            Parameters:
+            Returns:
+        '''
+        self.status["motion"] = "ready"
+
+    def wait_ready(self):
+        '''
+        Sets the robot motion to "ready"
+            Parameters:
+            Returns:
+        '''
+        rate = self.create_rate(2) #2Hz
+        while self.status["motion"] != "ready":
+            rate.sleep()
+            pass
 
     def move_callback(self, msg):
+        '''
+        Callback for axidraw_move topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received move: {}".format(msg))
@@ -83,6 +156,11 @@ class AxidrawSerial(Node):
         self.set_ready()
 
     def penup_callback(self, msg):
+        '''
+        Callback for axidraw_penup topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received penup: {}".format(msg))
@@ -91,6 +169,11 @@ class AxidrawSerial(Node):
         self.set_ready()
 
     def pendown_callback(self, msg):
+        '''
+        Callback for axidraw_pendown topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received pendown: {}".format(msg))
@@ -99,6 +182,11 @@ class AxidrawSerial(Node):
         self.set_ready()
 
     def stroke_callback(self, msg):
+        '''
+        Callback for axidraw_stroke topic
+            Parameters:
+            Returns:
+        '''
         self.set_busy()
 
         self.get_logger().info("Received path: {}".format(msg))
@@ -112,10 +200,11 @@ def main(args=None):
     rclpy.init(args=args)
 
     axidraw_serial = AxidrawSerial()
-
-    rclpy.spin(axidraw_serial)
-
-    rclpy.shutdown()
+    try:
+        rclpy.spin(axidraw_serial)
+    finally:
+        axidraw_serial.go_home()
+        rclpy.shutdown()
 
 
 if __name__ == '__main__':

src/draw_svg/CMakeLists.txt

@@ -12,7 +12,7 @@ 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)
+#find_package(moveit_visual_tools REQUIRED)
 
 # Install C++
 set(SRC_CPP_DIR src/cpp)

src/drawing_controller/drawing_controller/drawing_controller.py

@@ -17,6 +17,8 @@ from robot_interfaces.msg import Motion
 import sys
 from copy import deepcopy
 
+from drawing_controller.svg_processor import SVGProcessor
+
 def quaternion_from_euler(ai, aj, ak):
     ai /= 2.0
     aj /= 2.0
@@ -55,26 +57,37 @@ def map_point_function(x_pixels, y_pixels, xlim_lower, xlim_upper, ylim_lower, y
 
 
 class DrawingController(Node):
+
     def __init__(self, svgpath):
         super().__init__('drawing_controller')
         #self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10)
-        timer_period = 20.0  # 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')
 
         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)
-        self.lines = []
+        #self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2)
+        try:
+            self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.0, 1.0, 0.0, 1.0)
+        except:
+            self.map_point = map_point_function(1000, 1000, 0.0, 1.0, 0.0, 1.0)
+
+        self.lines = [ ((0,0),(0,0)) ]
         for child in svg:
             if (child.tag == 'line'):
                 p1 = (float(child.get('x1')), float(child.get('y1')))
                 p2 = (float(child.get('x2')), float(child.get('y2')))
                 self.lines.append((p1,p2))
 
+        self.svg_processor = SVGProcessor(self.get_logger())
+        print(self.svg_processor.process_svg(svgpath))
+
     def send_goal(self, motion):
+        self.busy = True
         goal_msg = ExecuteMotion.Goal()
         goal_msg.motion = motion
 
@@ -98,6 +111,7 @@ class DrawingController(Node):
     def get_result_callback(self, future):
         result = future.result().result
         self.get_logger().info('Result: {0}'.format(result))
+        self.busy = False
 
     def feedback_callback(self, feedback_msg):
         feedback = feedback_msg.feedback
@@ -120,15 +134,17 @@ class DrawingController(Node):
         motion.path.append(ps)
 
     def timer_callback(self):
+        if self.busy:
+            return
         next_line = self.lines[self.i]
         motion = Motion()
         p1 = self.map_point(next_line[0][0],next_line[0][1])
         p2 = self.map_point(next_line[1][0],next_line[1][1])
         self.get_logger().info('Drawing line with p1:{} p2:{}'.format(p1,p2))
-        self.append_point(motion, p1, 0.2)
-        self.append_point(motion, p1, 0.1)
-        self.append_point(motion, p2, 0.1)
-        self.append_point(motion, p2, 0.2)
+        self.append_point(motion, p1, 1.0)
+        self.append_point(motion, p1, 0.0)
+        self.append_point(motion, p2, 0.0)
+        self.append_point(motion, p2, 1.0)
         self.i = (self.i + 1) % len(self.lines)
 
         self.get_logger().info('Executing motion:{}'.format(motion.path))

src/drawing_controller/drawing_controller/svg_processor.py

@@ -0,0 +1,286 @@
+#!/usr/bin/env python3
+
+import lxml.etree as ET
+import splipy.curve_factory as cf
+import numpy as np
+
+class SVGProcessor():
+    """
+    Class which reads an svg file and creates sequences of points
+
+    ...
+
+    Attributes
+    ----------
+    primitive_fns : dict
+        contains a mapping of SVG primitive names to functions that produce points
+
+    Methods
+    -------
+
+    Services
+    -------
+
+    Topics
+    -------
+    """
+
+    def __init__(self, logger):
+        self.logger = logger
+        # A dict containing svg primitive names mapping to functions that handle them
+        self.primitive_fns = {
+            # Reference:
+            # https://developer.mozilla.org/en-US/docs/Web/SVG/Element
+            "line": self.primitive_line,
+            "polyline": self.primitive_polyline,
+            "polygon": self.primitive_polygon,
+        }
+        self.map_point = self.map_point_function(1000,
+                                                1000)
+
+    def get_primitive_fn(self, primitive):
+        '''
+        Looks up the primitive tag name in the dictionary of functions.
+
+            Parameters:
+                    primitive (lxml child): the primitive from the svg file
+            Returns:
+                    primitive_fn ():
+        '''
+        def log_error(p, _):
+           self.logger.error("'{}' not supported".format(p.tag))
+           return []
+        return self.primitive_fns.get(primitive.tag, log_error)
+
+
+    def down_and_up(self, points):
+        down = (points[0][0], points[0][1], 1)
+        up = (points[-1][0], points[-1][1], 1)
+
+        return [down] + points + [up]
+
+    def primitive_line(self, child):
+        p1 = self.map_point(float(child.get('x1')), float(child.get('y1')))
+        p2 = self.map_point(float(child.get('x2')), float(child.get('y2')))
+        return [
+            (p1[0],p1[1],0),
+            (p2[0],p2[1],0),
+            ]
+
+    def primitive_polyline(self, child):
+        points = child.get('points').split(' ')
+        points = [(self.map_point(float(p[0]),float(p[1]))) for p in points.split(',')]
+        output = []
+        for p in points:
+            output.append((p[0],p[1],0))
+        return output
+
+    def primitive_polygon(self, child):
+        output = self.primitive_polyline(child)
+        output.append((output[0][0],output[0][1],0))
+        return output
+
+    # https://developer.mozilla.org/en-US/docs/Web/SVG/Attribute/d
+    def path_parser(self, child):
+        '''
+
+        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
+        Elliptical Arc Curve: A, a
+        ClosePath: Z, z
+
+            Parameters:
+                    primitive (lxml child): the primitive from the svg file
+            Returns:
+                    primitive_fn ():
+        '''
+        pathstr = child.get('d')
+
+        # Tokenizer
+        self.logger.info("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr)))
+        path = []
+        i = 0
+        while i < len(pathstr):
+            c = pathstr[i]
+            # Single letter commands
+            if c.isalpha():
+                path.append(c)
+
+            # Numbers
+            if c == '-' or c.isdecimal():
+                s = ""
+                while i < len(pathstr) and not c.isspace():
+                    s = s + c
+                    i += 1
+                    c = pathstr[i]
+                path.append(s)
+            i += 1
+
+        # Parser
+        self.logger.info("Parsing path :'{}...' with {} tokens".format(path[:20], len(path)))
+        x = 0.0
+        y = 0.0
+        i = 0
+        output = []
+        def getnum():
+            nonlocal i
+            i += 1
+            return float(path[i])
+        def nextisnum():
+            return path[i+1].isdecimal()
+        def setpointup():
+            nonlocal output
+            p = self.map_point(x,y)
+            output.append((p[0],p[1],1.0))
+        def setpointdown():
+            nonlocal output
+            p = self.map_point(x,y)
+            output.append((p[0],p[1],0.0))
+        def appendpoints(points):
+            nonlocal output
+            for x,y in points:
+                p = self.map_point(x,y)
+                output.append((p[0],p[1],0.0))
+
+        while i < len(path):
+            w = path[i]
+            # MoveTo commands
+            if (w == "M"):
+                setpointup()
+                x = getnum()
+                y = getnum()
+                setpointup()
+                i += 1
+                continue
+            if (w == "m"):
+                setpointup()
+                x += getnum()
+                y += getnum()
+                setpointup()
+                i += 1
+                continue
+            # LineTo commands
+            if (w == "L"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "l"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "H"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "h"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "V"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "v"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            # Cubic Bézier Curve commands
+            if (w == "C"):
+                self.logger.info("SVG path parser cubic bezier curve at i={}".format(i))
+                while True:
+                    # https://github.com/sintef/Splipy/tree/master/examples
+                    control_points = [(x,y),
+                                      (getnum(),getnum()),
+                                      (getnum(),getnum()),
+                                      (getnum(),getnum())]
+                    x = control_points[-1][0]
+                    y = control_points[-1][1]
+                    control_points = np.array(control_points)
+                    n = 10
+                    curve = cf.cubic_curve(control_points)
+                    lin = np.linspace(curve.start(0), curve.end(0), n)
+                    coordinates = curve(lin)                                 # physical (x,y)-coordinates, size (n,2)
+                    appendpoints(coordinates)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
+            if (w == "c"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "S"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "s"):
+                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))
+            if (w == "q"):
+                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"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            # Elliptical arc commands
+            if (w == "A"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "a"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            # ClosePath commands
+            if (w == "Z"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            if (w == "z"):
+                self.logger.error("SVG path parser '{}' not implemented".format(w))
+            self.logger.error("SVG path parser panic mode at '{}'".format(w))
+
+            i += 1
+        self.logger.info("Finished parsing path")
+        return output
+
+    # https://stackoverflow.com/questions/30232031/how-can-i-strip-namespaces-out-of-an-lxml-tree
+    def strip_ns_prefix(self, tree):
+        #xpath query for selecting all element nodes in namespace
+        query = "descendant-or-self::*[namespace-uri()!='']"
+        #for each element returned by the above xpath query...
+        for element in tree.xpath(query):
+            #replace element name with its local name
+            element.tag = ET.QName(element).localname
+        return tree
+
+    def process_svg(self, svg_path):
+        with open(svg_path) as svg_file:
+            xml = ET.parse(svg_file)
+            svg = xml.getroot()
+            svg = self.strip_ns_prefix(svg)
+
+            if 'width' in svg.attrib:
+                self.map_point = self.map_point_function(float(svg.get('width')),
+                                                         float(svg.get('height')))
+            elif 'viewBox' in svg.attrib:
+                # TODO parse viewBox
+                pass
+            else:
+               self.logger.error("Unable to get SVG dimensions")
+
+            motions = []
+            for child in svg:
+               self.logger.info("Attempting to process SVG primitive:'{}'".format(child.tag))
+               primitive_fn = self.primitive_line
+               # path can consist of multiple primitives
+               if (child.tag == 'path'):
+                   for m in self.path_parser(child):
+                       motions.append(m)
+               else:
+                   primitive_fn = self.get_primitive_fn(child)
+                   motions.append(primitive_fn(child))
+
+            motions_refined = []
+            for m in motions:
+                if m == []:
+                    continue
+                self.logger.info("Refining:'{}'".format(m))
+                motions_refined.append(self.down_and_up(m))
+            return motions_refined
+
+    def translate(self, 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(self, x_pixels, y_pixels):
+        def map_point(xpix,ypix):
+            x = self.translate(xpix, 0, x_pixels, 0, 1)
+            y = self.translate(ypix, 0, y_pixels, 0, 1)
+            return (x,y)
+        return map_point
+

src/lite6_controller/CMakeLists.txt

@@ -37,6 +37,7 @@ endif()
 add_executable(lite6_controller src/lite6_controller.cpp)
 ament_target_dependencies(lite6_controller
   "rclcpp"
+  "moveit"
   "rclcpp_action"
   "Eigen3"
   "pilz_industrial_motion_planner"

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -16,7 +16,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='warn')
+    log_level = LaunchConfiguration("log_level", default='info')
     rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz"))
 
     prefix = LaunchConfiguration('prefix', default='')
@@ -299,21 +299,29 @@ 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)
 
-    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')
+    # 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'] = 0.5
 
-        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'])
+    #print(joint_limits_yaml)
+    #quit()
+
+    #if add_gripper.perform(context) in ('True', 'true'):
+    #    gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
+    #    gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')
+    #    gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml')
+
+    #    if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml:
+    #        for name in gripper_controllers_yaml['controller_names']:
+    #            if name in gripper_controllers_yaml:
+    #                if name not in controllers_yaml['controller_names']:
+    #                    controllers_yaml['controller_names'].append(name)
+    #                controllers_yaml[name] = gripper_controllers_yaml[name]
+    #    if gripper_ompl_planning_yaml:
+    #        ompl_planning_yaml.update(gripper_ompl_planning_yaml)
+    #    if joint_limits_yaml and gripper_joint_limits_yaml:
+    #        joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
 
     add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
     add_prefix_to_moveit_params(
@@ -325,22 +333,22 @@ def launch_setup(context, *args, **kwargs):
 
     # Planning pipeline
     # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py
-    planning_pipeline = {
-        "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"],
-        "default_planning_pipeline": "pilz_industrial_motion_planner",
-        "ompl": {
-            "planning_plugin": "ompl_interface/OMPLPlanner",
-            # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved
-            "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
-            # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled
-            "start_state_max_bounds_error": 0.31416,
-        },
-        "pilz_industrial_motion_planner": {
-            "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner",
-            "default_planner_config": "PTP",
-            "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
-        },
-    }
+    #planning_pipeline = {
+    #    "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"],
+    #    "default_planning_pipeline": "pilz_industrial_motion_planner",
+    #    "ompl": {
+    #        "planning_plugin": "ompl_interface/OMPLPlanner",
+    #        # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved
+    #        "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
+    #        # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled
+    #        "start_state_max_bounds_error": 0.31416,
+    #    },
+    #    "pilz_industrial_motion_planner": {
+    #        "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner",
+    #        "default_planner_config": "PTP",
+    #        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
+    #    },
+    #}
     # Kinematics
     #kinematics = load_yaml('panda_moveit2_config', 'config/kinematics.yaml')
 
@@ -431,7 +439,8 @@ def launch_setup(context, *args, **kwargs):
         arguments=['-d', rviz_config_file],
         parameters=[
             robot_description_parameters,
-            ompl_planning_pipeline_config,
+            #ompl_planning_pipeline_config,
+            pilz_planning_pipeline_config,
             {'use_sim_time': use_sim_time},
         ],
         remappings=[
@@ -455,7 +464,7 @@ def launch_setup(context, *args, **kwargs):
         #    target_action=rviz2_node,
         #    on_exit=[EmitEvent(event=Shutdown())]
         #)),
-        rviz2_node,
+        #rviz2_node,
         static_tf,
         move_group_node,
         robot_gazebo_launch,

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -155,6 +155,11 @@ 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)
 
+    # 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'] = 0.5
+
     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')
@@ -187,6 +192,16 @@ def launch_setup(context, *args, **kwargs):
     }
     ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
 
+
+    pilz_planning_pipeline_config = {
+        'move_group': {
+            '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""",
+            "default_planner_config": "PTP",
+            "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
+        }
+    }
+
     # Moveit controllers Configuration
     moveit_controllers = {
         moveit_controller_manager_key.perform(context): controllers_yaml,
@@ -229,7 +244,8 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
-            ompl_planning_pipeline_config,
+            #ompl_planning_pipeline_config,
+            pilz_planning_pipeline_config,
             trajectory_execution,
             plan_execution,
             moveit_controllers,

src/lite6_controller/src/lite6_controller.cpp

@@ -1,68 +1,133 @@
 #include <cstdio>
 #include <rclcpp/rclcpp.hpp>
 #include <robot_controller/robot_controller.hpp>
+#include <chrono>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit_msgs/msg/collision_object.hpp>
 
-#include <moveit/planning_interface/planning_interface.h>
-#include <moveit/planning_interface/planning_response.h>
+//#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/motion_plan_request.hpp>
 #include <moveit_msgs/msg/motion_sequence_request.hpp>
 #include <moveit_msgs/msg/motion_sequence_item.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;
 
 // 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
+ */
 class Lite6Controller : public RobotController
 {
 public:
-  /// Move group interface for the robot
+  /**
+   * Move group interface for the robot
+   */
   moveit::planning_interface::MoveGroupInterface move_group;
 
+  /**
+   * TODO Use instead of MoveGroupInterface
+   * https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
+   */
+  moveit_cpp::MoveItCppPtr moveit_cpp_;
+  moveit_cpp::PlanningComponentPtr planning_component_;
+  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+
   //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
+   */
   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())
   {
 
     //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.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));
 
+  }
+
+  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");
+    }
+    catch (int e) {
+      RCLCPP_ERROR(this->get_logger(), "Initialization Failed: %d", e);
+      return;
+    }
+
+
     move_group.setPlanningTime(30.0);
     RCLCPP_INFO(this->get_logger(), "Initialization successful.");
 
   }
 
+  /**
+   * This function takes a pose and returns a MotionPlanRequest
+   */
   planning_interface::MotionPlanRequest createRequest(geometry_msgs::msg::PoseStamped pose)
   {
     planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
@@ -74,9 +139,15 @@ public:
     std::vector<double> tolerance_pose(3, 0.01);
     std::vector<double> tolerance_angle(3, 0.01);
 
+
+    // Set motion goal of end effector link
+    //std::string ee_link = moveit_cpp_->getRobotModel()->getJointModelGroup(planning_component_->getPlanningGroupName())->getLinkModelNames().back();
+    //RCLCPP_INFO(this->get_logger(), "Got ee_link");
+
     mpr.group_name = MOVE_GROUP;
     moveit_msgs::msg::Constraints pose_goal =
       kinematic_constraints::constructGoalConstraints("link_eef", pose, tolerance_pose, tolerance_angle);
+      //kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle);
 
     mpr.goal_constraints.push_back(pose_goal);
     return mpr;
@@ -89,8 +160,93 @@ public:
 
   // TODO implement feedback
   // https://answers.ros.org/question/249995/how-to-check-sate-of-plan-execution-in-moveit-during-async-execution-in-python/
+  //
+  // Useful
+  // https://groups.google.com/g/moveit-users/c/I4sPhq_JGQk
+  // https://groups.google.com/g/moveit-users/c/MOoFxy2exT4/m/0AwRHOuEwRgJ
+  // https://discourse.ros.org/t/moveit-trajectory-through-waypoints/17439
+  // https://answers.ros.org/question/330632/moveit-motion-planning-how-should-i-splice-several-planned-motion-trajectories/
+  // https://groups.google.com/g/moveit-users/c/lZL2HTjLu-k
+  //
 
-  /// Callback that executes path on robot
+  // Set limits for A4 paper
+  float xlim_lower = 0.15;
+  float xlim_upper = 0.3;
+  float ylim_lower = -0.075;
+  float ylim_upper = 0.075;
+  float zlim_lower = 0.1;
+  float zlim_upper = 0.15;
+
+  /**
+   * Function that translates an input value with a given range to a value within another range.
+   */
+  float translate(float val, float lmin, float lmax, float rmin, float rmax)
+  {
+    float lspan = lmax - lmin;
+    float rspan = rmax - rmin;
+    val = (val - lmin) / lspan;
+    return rmin + (val * rspan);
+  }
+
+  /**
+   * Translates a pose to the xArm coordinate frame
+   */
+  geometry_msgs::msg::PoseStamped translatePose(geometry_msgs::msg::PoseStamped pose)
+  {
+    // TODO support paper angle
+    pose.pose.position.x = translate(pose.pose.position.x, 0, 1, xlim_lower, xlim_upper);
+    pose.pose.position.y = translate(pose.pose.position.y, 0, 1, ylim_lower, ylim_upper);
+    pose.pose.position.z = translate(pose.pose.position.z, 0, 1, zlim_lower, zlim_upper);
+
+    return pose;
+  }
+
+  void logPose(geometry_msgs::msg::PoseStamped pose)
+  {
+    RCLCPP_INFO(this->get_logger(), "pose position.x: %f", pose.pose.position.x);
+    RCLCPP_INFO(this->get_logger(), "pose position.y: %f", pose.pose.position.y);
+    RCLCPP_INFO(this->get_logger(), "pose position.z: %f", pose.pose.position.z);
+  }
+
+  /**
+   * Creates a trajectory for a pose and appends it to a given trajectory
+   */
+  void addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory)
+  {
+    pose = translatePose(pose);
+    move_group.setPoseTarget(pose);
+    //moveit_msgs::msg::RobotTrajectory trajectory;
+    //move_group.setPlanningPipelineId("PTP");
+    move_group.setPlannerId("PTP");
+
+    robot_trajectory::RobotTrajectory previous_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
+    previous_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), *trajectory);
+
+
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
+    RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED");
+
+    robot_trajectory::RobotTrajectory next_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
+    next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
+
+    // append trajectory, with time step of 2.0, not skipping any points
+    previous_trajectory.append(next_trajectory, 2.0, 0);
+    *trajectory = moveit_msgs::msg::RobotTrajectory();
+    previous_trajectory.getRobotTrajectoryMsg(*trajectory);
+
+    // Append segment to complete trajectory
+    //trajectory->joint_trajectory.points.insert(trajectory->joint_trajectory.points.end(),
+    //                                    plan.trajectory_.joint_trajectory.points.begin(),
+    //                                    plan.trajectory_.joint_trajectory.points.end());
+    //trajectory->joint_trajectory.joint_names = plan.trajectory_.joint_trajectory.joint_names;
+
+    move_group.clearPoseTarget();
+  }
+
+  /**
+   * Callback that executes path on robot
+   */
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
 
@@ -100,22 +256,60 @@ public:
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
 
-    moveit_msgs::msg::MotionSequenceRequest msr;
-    //waypoints.push_back(move_group.getCurrentPose().pose);
+    // getting current state of robot from environment
+    //if (!moveit_cpp_->getPlanningSceneMonitor()->requestPlanningSceneState())
+    //{
+    //  RCLCPP_ERROR(this->get_logger(), "Failed to get planning scene");
+    //  return;
+    //}
+    //moveit::core::RobotStatePtr start_robot_state = moveit_cpp_->getCurrentState(2.0);
+
+    //moveit_msgs::msg::MotionSequenceRequest msr;
+    ////waypoints.push_back(move_group.getCurrentPose().pose);
+    //for (auto p : goal->motion.path)
+    //{
+    //  RCLCPP_INFO(this->get_logger(), "Creating MSI");
+    //  moveit_msgs::msg::MotionSequenceItem msi;
+    //  msi.req = createRequest(p);
+    //  msi.blend_radius = 0.001; //TODO make configurable
+    //  msr.items.push_back(msi);
+    //}
+    //RCLCPP_INFO(this->get_logger(), "Created MSR");
+
+    moveit_msgs::msg::RobotTrajectory multi_trajectory;
+    //robot_trajectory::RobotTrajectory rt(move_group.getRobotModel(), MOVE_GROUP);
+    robot_trajectory::RobotTrajectory single_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
+
+    move_group.setStartStateToCurrentState();
     for (auto p : goal->motion.path)
     {
-      moveit_msgs::msg::MotionSequenceItem msi;
-      msi.req = createRequest(p);
-      msi.blend_radius = 0.001; //TODO make configurable
-      msr.items.push_back(msi);
+      //RCLCPP_INFO(this->get_logger(), "Planning trajectory");
+
+      // Append next pose to trajectory
+      addPoseToTrajectory(p, &multi_trajectory);
+
+      // set move_group start state to final pose of trajectory
+      //RCLCPP_INFO(this->get_logger(), "setRobotTrajectoryMsg");
+      single_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), multi_trajectory);
+      //rt.setRobotTrajectoryMsg(rt.getLastWayPoint(), trajectory);
+      //RCLCPP_INFO(this->get_logger(), "getLastWayPoint");
+      moveit::core::RobotState robot_state = single_trajectory.getLastWayPoint();
+      //RCLCPP_INFO(this->get_logger(), "eef");
+      //const Eigen::Isometry3d& eef_transform = robot_state.getGlobalLinkTransform(move_group.getEndEffectorLink());
+      //geometry_msgs::msg::Pose pose;
+      //pose = Eigen::toMsg(eef_transform);
+      move_group.setStartState(robot_state);
+
+
+      //trajectory = moveit_msgs::msg::RobotTrajectory();
     }
 
-    moveit_msgs::msg::RobotTrajectory trajectory;
+    RCLCPP_INFO(this->get_logger(), "Executing trajectory with %ld points", multi_trajectory.joint_trajectory.points.size());
+    this->move_group.execute(multi_trajectory);
 
     //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();
 
@@ -145,6 +339,9 @@ public:
   }
 };
 
+/**
+ * Starts lite6_controller
+ */
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
@@ -152,7 +349,15 @@ int main(int argc, char ** argv)
   RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Starting lite6_controller");
   auto lite6 = std::make_shared<Lite6Controller>();
 
-  rclcpp::executors::SingleThreadedExecutor executor;
+  // TODO remove sleep if not necessary
+  // Sleep in case move_group not loaded
+  rclcpp::sleep_for(2s);
+
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Setting up lite6_controller");
+  lite6->setup();
+
+  //rclcpp::executors::SingleThreadedExecutor executor;
+  rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(lite6);
   executor.spin();
 

src/robot_controller/include/robot_controller/robot_controller.hpp

@@ -6,6 +6,9 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
 
+/**
+ * Class which creates the ExecuteMotion action which the drawing_controller uses to talk to the robots.
+ */
 class RobotController : public rclcpp::Node
 {
 public:
@@ -26,13 +29,15 @@ private:
 
   virtual void motion_handle_accepted(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle);
 
-  /// Callback that executes path on robot
   virtual void executePath(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle);
 };
 
 using ExecuteMotion = robot_interfaces::action::ExecuteMotion;
 using GoalHandleExecuteMotion = rclcpp_action::ServerGoalHandle<ExecuteMotion>;
 
+/**
+ *
+ */
 RobotController::RobotController(const rclcpp::NodeOptions & options)
 : Node("robot_controller",options)
 {
@@ -46,6 +51,9 @@ RobotController::RobotController(const rclcpp::NodeOptions & options)
     std::bind(&RobotController::motion_handle_accepted, this, _1));
 }
 
+/**
+ *
+ */
 rclcpp_action::GoalResponse RobotController::motion_handle_goal(
   const rclcpp_action::GoalUUID & uuid,
   std::shared_ptr<const ExecuteMotion::Goal> goal)
@@ -55,6 +63,9 @@ rclcpp_action::GoalResponse RobotController::motion_handle_goal(
   return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }
 
+/**
+ *
+ */
 rclcpp_action::CancelResponse RobotController::motion_handle_cancel(
   const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
 {
@@ -63,6 +74,9 @@ rclcpp_action::CancelResponse RobotController::motion_handle_cancel(
   return rclcpp_action::CancelResponse::ACCEPT;
 }
 
+/**
+ *
+ */
 void RobotController::motion_handle_accepted(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
 {
   using namespace std::placeholders;
@@ -70,7 +84,9 @@ void RobotController::motion_handle_accepted(const std::shared_ptr<GoalHandleExe
   std::thread{std::bind(&RobotController::executePath, this, _1), goal_handle}.detach();
 }
 
-/// Callback that executes path on robot
+/**
+ * Callback that executes path on robot
+ */
 void RobotController::executePath(const std::shared_ptr<GoalHandleExecuteMotion> goal_handle)
 {
   const auto goal = goal_handle->get_goal();

src/robot_controller/src/cpp/dummy_controller.cpp

@@ -9,13 +9,17 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
 
-// A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
+/**
+ * A controller for a Dummy robot. Only logs messages and serves as an example for real implementation.
+ */
 class DummyController : public RobotController
 {
   public:
   DummyController(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) : RobotController(options) {}
 
-  /// Callback that executes path on robot
+  /**
+   * Callback that executes path on robot
+   */
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
     RCLCPP_INFO(this->get_logger(), "Executing goal");
@@ -58,6 +62,9 @@ class DummyController : public RobotController
   }
 };
 
+/**
+ *
+ */
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);

svg/test.svg

@@ -0,0 +1,7 @@
+<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>