Adjust lite6 params

Dockerfile

@@ -75,7 +75,7 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
     rm -rf ${WS_LOG_DIR}
 
 # Copy example svg images
-COPY ./svg svg
+COPY ./svg test-images
 
 ### Add workspace to the ROS entrypoint
 ### Source ROS workspace inside `~/.bashrc` to enable autocompletion

README.md

@@ -35,6 +35,21 @@ 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:/ws/PATH_IN_CONTAINER:ro
+```
+
+#### Podman issues
+If using podman instead of docker, using the following will allow the container to access `/dev/` which is needed by the axidraw robot. 
+``` sh
+sudo bash .docker/build.bash
+```
+``` sh
+sudo bash .docker/devel.bash
+```
+Adding sudo may cause gazebo not to work, so it is recommended to use docker instead of podman.
+
 ## TODO Building locally
 
 Requirements:
@@ -60,9 +75,11 @@ DummyController echoes Motion messages to the terminal.
 ros2 run robot_controller dummy_controller
 ```
 
-AxidrawController draws on the axidraw robot
+AxidrawController draws on the axidraw robot.
+Find the serial device in "/dev/", it is usually named "/dev/ttyACMX" where X is usually 0.
+Try a different serial port if the axidraw_controller continuously logs a message about failing to connect.
 ``` sh
-ros2 launch axidraw_controller axidraw_controller
+ros2 launch axidraw_controller axidraw_controller.launch.py serial_port:=/dev/ttyACM0
 ```
 
 This starts the simulated lite6
@@ -87,6 +104,23 @@ ros2 run drawing_controller drawing_controller svg/test.svg
 ```
 This will draw the svg image given as the last argument.
 
+### tmux workflow
+lite6 interface: http://192.168.1.150:18333
+
+#### Raspberry pi
+On the raspberry pi run 
+``` sh
+./setup_ros.sh
+```
+This will open a tmux session with the necessary ros2 packages sourced.
+#### Docker container
+``` sh
+tmux
+```
+
+If actively
+
+
 ## SVG compatibility info
 Tested with SVG from the following programs
 - Inkscape
@@ -177,7 +211,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             |             |
 

src/axidraw_controller/src/cpp/axidraw_controller.cpp

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

src/axidraw_controller/src/py/axidraw_serial.py

@@ -42,6 +42,7 @@ class AxidrawSerial(Node):
     status = {
         "serial": "not ready",
         "motion": "waiting",
+        "pen": "up",
     }
 
     def init_serial(self, port):
@@ -60,9 +61,17 @@ 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 = 100  # 100% speed
+        self.ad.options.speed_penup = 100    # 100% speed
+        self.ad.options.accel = 100          # 100% speed
+        self.ad.options.pen_rate_lower = 100 # 100% speed
+        self.ad.options.pen_rate_raise = 100 # 100% speed
+        #self.ad.options.pen_delay_down = 50
+        #self.ad.options.pen_delay_up = 50
         self.ad.update()                     # Process changes to options
         self.status["serial"] = "ready"
         self.status["motion"] = "ready"
+        self.status["pen"] = "up"
         return True
 
     def __init__(self):
@@ -83,7 +92,8 @@ class AxidrawSerial(Node):
         self.status_srv = self.create_service(Status, 'axidraw_status', self.get_status)
 
         while not self.init_serial(port):
-            self.get_logger().error("Failed to connect to axidraw on port:{}".format(port))
+            self.get_logger().error("Failed to connect to axidraw on port:{}, retrying".format(port))
+        self.get_logger().info("Successfully connected to axidraw on port:{}".format(port))
 
         self.move_sub = self.create_subscription(Point, 'axidraw_move', self.move_callback, qos_profile=QoSProfile(depth=1))
         self.penup_sub = self.create_subscription(Empty, 'axidraw_penup', self.penup_callback, qos_profile=QoSProfile(depth=1))
@@ -165,7 +175,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received penup: {}".format(msg))
 
-        self.ad.penup()
+        if self.status['pen'] == "down":
+            self.ad.penup()
+            self.status['pen'] = "up"
         self.set_ready()
 
     def pendown_callback(self, msg):
@@ -178,7 +190,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received pendown: {}".format(msg))
 
-        self.ad.pendown()
+        if self.status['pen'] == "up":
+            self.ad.pendown()
+            self.status['pen'] = "down"
         self.set_ready()
 
     def stroke_callback(self, msg):
@@ -193,6 +207,7 @@ class AxidrawSerial(Node):
 
         path = [ [p.x,p.y] for p in msg.points ]
         self.ad.draw_path(path)
+        self.status['pen'] = "up"
         self.set_ready()
 
 

src/custom_xarm_moveit_config/config/lite6/cartesian_limits.yaml

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

src/custom_xarm_moveit_config/config/lite6/joint_limits.yaml

@@ -4,31 +4,31 @@
 joint_limits:
   joint1:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 30.0
   joint2:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 30.0
   joint3:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 30.0
   joint4:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 30.0
   joint5:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 30.0
   joint6:
     has_velocity_limits: true
-    max_velocity: 2.14
+    max_velocity: 4.14
     has_acceleration_limits: true
-    max_acceleration: 1.0
+    max_acceleration: 30.0

src/custom_xarm_moveit_config/launch/lib/robot_moveit_config_lib.py

@@ -65,19 +65,22 @@ def add_prefix_to_moveit_params(controllers_yaml=None, ompl_planning_yaml=None,
 
 
 def get_xarm_robot_description_parameters(
-    xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-    xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+    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']),
     urdf_arguments={},
     srdf_arguments={},
     arguments={}):
     urdf_arguments['ros2_control_plugin'] = urdf_arguments.get('ros2_control_plugin', 'uf_robot_hardware/UFRobotSystemHardware')
-    moveit_config_package_name = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     xarm_type = arguments.get('xarm_type', None)
     
     # xarm_description/launch/lib/robot_description_lib.py
-    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
+    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('custom_xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
     get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
-    
+
+    joint_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml')
+    cartesian_limits = load_yaml(moveit_config_package_name, 'config', xarm_type, 'cartesian_limits.yaml')
+    joint_limits['cartesian_limits'] = cartesian_limits['cartesian_limits']
     return {
         'robot_description': get_xacro_file_content(
             xacro_file=xacro_urdf_file, 
@@ -87,6 +90,6 @@ def get_xarm_robot_description_parameters(
             xacro_file=xacro_srdf_file,
             arguments=srdf_arguments
         ),
-        'robot_description_planning': load_yaml(moveit_config_package_name, 'config', xarm_type, 'joint_limits.yaml'),
+        'robot_description_planning': joint_limits,
         'robot_description_kinematics': load_yaml(moveit_config_package_name, 'config', xarm_type, 'kinematics.yaml')
     }

src/drawing_controller/drawing_controller/svg_path_parser.py

@@ -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 = 50
-            curve = cf.cubic_curve(control_points)
+            #control_points = control_points / maxval #normalize values
+            n = 100
+            curve = cf.bezier(control_points)
             lin = np.linspace(curve.start(0), curve.end(0), n)
             coordinates = curve(lin)
             coordinates = np.nan_to_num(coordinates)
-            coordinates = coordinates * maxval #denormalize values
+            #coordinates = coordinates * maxval #denormalize values
             coordinates = dropzeros(coordinates)
             #self.logger.info("Appending curve points: {}".format(coordinates))
             #print(coordinates)
@@ -136,6 +136,20 @@ class SVGPathParser():
                 x = coordinates[-1][0]
                 y = coordinates[-1][1]
             return coordinates
+        def quadratic_bezier(control_points):
+            nonlocal x
+            nonlocal y
+            control_points = np.array(control_points)
+            n = 100
+            curve = cf.bezier(control_points, quadratic=True)
+            lin = np.linspace(curve.start(0), curve.end(0), n)
+            coordinates = curve(lin)
+            coordinates = np.nan_to_num(coordinates)
+            coordinates = dropzeros(coordinates)
+            if len(coordinates) > 0:
+                x = coordinates[-1][0]
+                y = coordinates[-1][1]
+            return coordinates
 
         while i < len(path):
             w = path[i]
@@ -208,7 +222,6 @@ class SVGPathParser():
             # Cubic Bézier Curve commands
             if (w == "C"):
                 while True:
-                    # https://github.com/sintef/Splipy/tree/master/examples
                     control_points = [(x,y),
                                       (getnum(),getnum()),
                                       (getnum(),getnum()),
@@ -221,7 +234,6 @@ class SVGPathParser():
                 continue
             if (w == "c"):
                 while True:
-                    # https://github.com/sintef/Splipy/tree/master/examples
                     control_points = [(x,y),
                                       (x + getnum(), y + getnum()),
                                       (x + getnum(), y + getnum()),
@@ -238,9 +250,27 @@ class SVGPathParser():
                 self.logger.error("SVG path parser '{}' not implemented".format(w))
             # Quadratic Bézier Curve commands
             if (w == "Q"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    control_points = [(x,y),
+                                      (getnum(),getnum()),
+                                      (getnum(),getnum())]
+                    coordinates = quadratic_bezier(control_points)
+                    appendpoints(coordinates)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "q"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    control_points = [(x,y),
+                                      (x + getnum(), y + getnum()),
+                                      (x + getnum(), y + getnum())]
+                    coordinates = quadratic_bezier(control_points)
+                    appendpoints(coordinates)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "T"):
                 self.logger.error("SVG path parser '{}' not implemented".format(w))
             if (w == "t"):

src/drawing_controller/drawing_controller/svg_processor.py

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

src/lite6_controller/CMakeLists.txt

@@ -16,6 +16,8 @@ find_package(moveit REQUIRED)
 find_package(pilz_industrial_motion_planner REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(xarm_api REQUIRED)
+find_package(xarm_msgs REQUIRED)
 
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -44,8 +46,11 @@ ament_target_dependencies(lite6_controller
   "robot_controller"
   "moveit_ros_planning_interface"
   "robot_interfaces")
-add_executable(lite6_trajectory_executor src/lite6_trajectory_executor.cpp)
-ament_target_dependencies(lite6_trajectory_executor
+
+add_executable(lite6_calibration src/lite6_calibration.cpp)
+ament_target_dependencies(lite6_calibration
+  "xarm_msgs"
+  "xarm_api"
   "rclcpp"
   "moveit"
   "rclcpp_action"
@@ -62,7 +67,7 @@ endif()
 
 install(TARGETS
   lite6_controller
-  lite6_trajectory_executor
+  lite6_calibration
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -17,7 +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("draw_svg"), "rviz", "ign_moveit2_examples.rviz"))
+    rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("custom_xarm_description"), "rviz", "display.rviz"))
 
     prefix = LaunchConfiguration('prefix', default='')
     hw_ns = LaunchConfiguration('hw_ns', default='xarm')
@@ -191,17 +191,6 @@ 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",
@@ -274,10 +263,14 @@ 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)
 
+    #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'
@@ -311,12 +304,8 @@ def launch_setup(context, *args, **kwargs):
         prefix=prefix.perform(context))
 
 
-    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
-
+    #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']
 
     # Planning pipeline
     # https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py
@@ -393,15 +382,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
-        #     "name": "planning_scene_monitor",
-        #     "robot_description": "robot_description",
-        #     "joint_state_topic": "/joint_states",
-        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
-        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
-        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
-        #     "wait_for_initial_state_timeout": 10.0,
-        # },
+        'publish_robot_description': True,
+        'publish_robot_description_semantic' :True,
     }
 
     # Start the actual move_group node/action server

src/lite6_controller/launch/lite6_real.launch.py

@@ -75,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('xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +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 = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
     get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
     robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        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']),
         urdf_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns.perform(context).strip('/'),
@@ -154,17 +154,12 @@ def launch_setup(context, *args, **kwargs):
     ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
 
-    robot_description_parameters['cartesian_limits'] = {}
-    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'] = '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)
@@ -186,11 +181,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(
@@ -232,15 +227,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
-        #     "name": "planning_scene_monitor",
-        #     "robot_description": "robot_description",
-        #     "joint_state_topic": "/joint_states",
-        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
-        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
-        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
-        #     "wait_for_initial_state_timeout": 10.0,
-        # },
+        'publish_robot_description': True,
+        'publish_robot_description_semantic' :True,
     }
 
 
@@ -249,7 +237,8 @@ def launch_setup(context, *args, **kwargs):
             'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
             #'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
             'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
-            "default_planner_config": "PTP",
+            #"default_planner_config": "PTP",
+            "default_planner_config": "LIN",
         }
     }
 
@@ -264,6 +253,7 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
+            #cartesian_limits,
             #ompl_planning_pipeline_config,
             pilz_planning_pipeline_config,
             move_group_capabilities,

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -75,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('xarm_description'), 'launch', '_robot_description.launch.py'])),
+        PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'launch', '_robot_description.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -105,12 +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 = 'xarm_moveit_config'
+    moveit_config_package_name = 'custom_xarm_moveit_config'
     mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py'))
     get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
     robot_description_parameters = get_xarm_robot_description_parameters(
-        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
-        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
+        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']),
         urdf_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns.perform(context).strip('/'),
@@ -156,9 +156,9 @@ def launch_setup(context, *args, **kwargs):
     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'] = 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)))
@@ -182,11 +182,8 @@ def launch_setup(context, *args, **kwargs):
         kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
         prefix=prefix.perform(context))
 
-    robot_description_parameters['cartesian_limits'] = {}
-    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
+    #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']
 
     # Planning Configuration
     ompl_planning_pipeline_config = {
@@ -236,15 +233,8 @@ def launch_setup(context, *args, **kwargs):
         'publish_geometry_updates': True,
         'publish_state_updates': True,
         'publish_transforms_updates': True,
-        # "planning_scene_monitor_options": {
-        #     "name": "planning_scene_monitor",
-        #     "robot_description": "robot_description",
-        #     "joint_state_topic": "/joint_states",
-        #     "attached_collision_object_topic": "/move_group/planning_scene_monitor",
-        #     "publish_planning_scene_topic": "/move_group/publish_planning_scene",
-        #     "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
-        #     "wait_for_initial_state_timeout": 10.0,
-        # },
+        'publish_robot_description': True,
+        'publish_robot_description_semantic' :True,
     }
 
     # Start the actual move_group node/action server

src/lite6_controller/package.xml

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

src/lite6_controller/src/lite6_calibration.cpp

@@ -0,0 +1,131 @@
+#include <signal.h>
+#include <chrono>
+#include <rclcpp/rclcpp.hpp>
+#include <xarm_api/xarm_ros_client.h>
+#include <iostream>
+#include <string>
+
+// MoveIt
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+
+
+const std::string MOVE_GROUP = "lite6";
+
+std::shared_ptr<rclcpp::Node> node;
+
+void exit_sig_handler(int signum)
+{
+    fprintf(stderr, "[lite6_calibration] Ctrl-C caught, exit process...\n");
+    exit(-1);
+}
+
+void wait()
+{
+ do
+ {
+   std::cout << "Press a key to continue...";
+ } while (std::cin.get() != '\n');
+}
+
+void println(std::string s)
+{
+  std::cout << s << std::endl;
+}
+
+void print_joints(std::vector<float> jnts)
+{
+    std::string out = "";
+    out = out + "{ ";
+    for (auto i : jnts)
+      out = out + std::to_string(i) + ", ";
+    out = out.substr(0, out.size()-2); //remove trailing comma
+    out = out + " }";
+    std::cout << out << std::endl;
+}
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    std::string hw_ns = "ufactory";
+    node = rclcpp::Node::make_shared("lite6_calibration");
+    int ret;
+
+    signal(SIGINT, exit_sig_handler);
+
+    xarm_api::XArmROSClient client;
+    client.init(node, hw_ns);
+    client.motion_enable(true);
+
+    println("Setting xArm lite6 to free-drive mode. Attach pen and touch the drawing surface.");
+    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("Moving to start drawing pose");
+    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");
+    wait();
+    return 0;
+}

src/lite6_controller/src/lite6_controller.cpp

@@ -47,15 +47,6 @@ const std::string MOVE_GROUP = "lite6";
 
 using namespace std::chrono_literals;
 
-// MOTION PLANNING API
-// https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp
-// https://moveit.picknik.ai/humble/doc/examples/motion_planning_api/motion_planning_api_tutorial.html
-// https://github.com/ros-planning/moveit2/blob/main/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp
-//
-//
-// USE
-// https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
-
 /**
  * Controller for xArm Lite6, implementing RobotController
  */
@@ -72,8 +63,6 @@ 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
@@ -91,20 +80,10 @@ public:
   float xlim_upper = 0.305;
   float ylim_lower = -0.1475;
   float ylim_upper = 0.1475;
-  float zlim_lower = 0.210;
-  float zlim_upper = 0.218;
+  //float zlim_lower = 0.1945;
+  float zlim_lower = 0.207493;
+  float zlim_upper = zlim_lower + 0.01;
 
-  //bool moved = false;
-  //
-  // TODO get pilz working
-  //std::unique_ptr<pilz_industrial_motion_planner::CommandListManager> command_list_manager_;
-  // command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
-
-  /**
-   * CommandListManager, used to plan MotionSequenceRequest
-   */
-  //pilz_industrial_motion_planner::CommandListManager command_list_manager;
-  //pilz_industrial_motion_planner::CommandListManager command_list_manager(*std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
 
   /**
    * Constructor
@@ -112,27 +91,11 @@ 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())
   {
-
-    //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));
 
@@ -140,18 +103,10 @@ 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");
@@ -222,11 +177,12 @@ public:
   /**
    *
    */
-  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path, std::string planner_id)
   {
     moveit_msgs::msg::MotionSequenceRequest msr = moveit_msgs::msg::MotionSequenceRequest();
     //waypoints.push_back(move_group.getCurrentPose().pose);
-    std::string ee_link = move_group.getLinkNames().back();
+    //std::string ee_link = move_group.getLinkNames().back();
+    std::string ee_link = "pen_link";
     RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str());
     geometry_msgs::msg::Point previous_point;
     //previous_point.point.x = -1.0;
@@ -238,13 +194,16 @@ public:
       moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
 
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
-      mpr.planner_id = "PTP";
+      mpr.planner_id = planner_id;
+      //mpr.planner_id = "PTP";
       //mpr.planner_id = "LIN";
       mpr.group_name = move_group.getName();
+      //mpr.max_velocity_scaling_factor = 1.0;
       mpr.max_velocity_scaling_factor = 1.0;
-      mpr.max_acceleration_scaling_factor = 0.98;
-      mpr.allowed_planning_time = 10;
-      mpr.max_cartesian_speed = 2; // m/s
+      mpr.max_acceleration_scaling_factor = 0.8;
+      //mpr.max_acceleration_scaling_factor = 0.1;
+      mpr.allowed_planning_time = 20;
+      //mpr.max_cartesian_speed = 2; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
 
       // A tolerance of 0.01 m is specified in position
@@ -302,6 +261,11 @@ public:
     return msr;
   }
 
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  {
+    return createMotionSequenceRequest(path, "LIN");
+  }
+
   /**
    *
    */
@@ -405,8 +369,17 @@ public:
 
   void executeTrajectoryFromQueue()
   {
-    if (busy || trajectory_queue.empty())
+    if (trajectory_queue.empty())
+    {
+      if (busy)
+        return;
+
+      busy = true;
+      //RCLCPP_INFO(this->get_logger(), "Getting robot state");
+      //move_group_state = move_group.getCurrentState(10);
+      busy = false;
       return;
+    }
     busy = true;
     RCLCPP_INFO(this->get_logger(), "Executing next trajectory from queue");
     move_group.execute(trajectory_queue.front());
@@ -414,6 +387,103 @@ 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
    */
@@ -426,38 +496,33 @@ public:
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
 
-    auto msr = createMotionSequenceRequest(&goal->motion.path);
-    RCLCPP_INFO(this->get_logger(), "Created MSR");
+    std::vector<moveit_msgs::msg::RobotTrajectory> ts;
+    pathToTrajectory(&goal->motion.path, &ts);
 
-    //planAndLogOffset(&goal->motion.path);
+    long unsigned int n = 0;
+
+    RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
+    for (auto t : ts)
+    {
+      RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
+      if (t.joint_trajectory.points.empty())
+      {
+        RCLCPP_ERROR(this->get_logger(), "TRAJECTORY IS EMPTY");
+        continue;
+      }
+      trajectory_queue.push(t);
+      n++;
+    }
 
-    RCLCPP_INFO(this->get_logger(), "Creating req");
-    auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
-    RCLCPP_INFO(this->get_logger(), "Setting msr request");
-    req->request = msr;
-    RCLCPP_INFO(this->get_logger(), "Sending request to sequence service");
-    auto res = sequence_client_->async_send_request(req);
-    RCLCPP_INFO(this->get_logger(), "Waiting for result");
-    auto ts = res.get()->response.planned_trajectories;
     std::string status = "";
-    if (ts.size() > 0)
+    if (n == ts.size())
     {
       status = "success";
-      RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
-      RCLCPP_INFO(this->get_logger(), "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;
@@ -477,27 +542,6 @@ 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
  */
@@ -518,7 +562,6 @@ 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();

src/lite6_controller/src/lite6_trajectory_executor.cpp

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

src/virtual_drawing_surface/CMakeLists.txt

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

src/virtual_drawing_surface/src/py/drawing_surface.py

@@ -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 = 4 # radius
+        r = 3 # 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.3485, 0.1, 0, self.height))
+            y = int(translate(p.x, -0.2485, 0.1, 0, self.height))
 
             #x = bound(self.width - x, 0, self.width)
             #y = bound(self.height - y, 0, self.height)