Add kinematics plugin settings

Dockerfile

@@ -39,38 +39,34 @@ 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 && \
-    rosdep update && \
-    apt-get update && \
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
-    rm -rf /var/lib/apt/lists/* && \
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+# Build interfaces and generic controller first
+COPY ./src/robot_interfaces ${WS_SRC_DIR}/robot_interfaces
+COPY ./src/robot_controller ${WS_SRC_DIR}/robot_controller
+RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
+    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/robot_interfaces ${WS_SRC_DIR}/robot_controller && \
     rm -rf ${WS_LOG_DIR}
 
-### Copy over the rest of ign_moveit2_examples, then install dependencies and build
-COPY ./ ${WS_SRC_DIR}/ign_moveit2_examples/
-RUN rosdep update && \
-    apt-get update && \
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
-    rm -rf /var/lib/apt/lists/* && \
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+# Build packages
+COPY ./src/draw_svg ${WS_SRC_DIR}/draw_svg
+COPY ./src/drawing_controller ${WS_SRC_DIR}/drawing_controller
+COPY ./src/axidraw_controller ${WS_SRC_DIR}/axidraw_controller
+RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
+    source "${WS_INSTALL_DIR}/local_setup.bash" && \
+    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/draw_svg ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller && \
     rm -rf ${WS_LOG_DIR}
 
-### Copy code built on top of example ign_moveit2_examples
-# TODO clean build process
-COPY ./src/* ${WS_SRC_DIR}/
-RUN rosdep update && \
-    apt-get update && \
-    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR} && \
-    rm -rf /var/lib/apt/lists/* && \
-    source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
-    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" && \
+# Build lite6 and xarm packages
+COPY ./src/lite6_controller ${WS_SRC_DIR}/lite6_controller
+RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
+    vcs import --recursive --shallow ${WS_SRC_DIR} < ${WS_SRC_DIR}/lite6_controller/lite6_controller.repos && \
+    mv ${WS_SRC_DIR}/xarm_ros2/xarm* ${WS_SRC_DIR} && \
+    rosdep install -y -r -i --rosdistro "${ROS_DISTRO}" --from-paths ${WS_SRC_DIR}/xarm_* && \
+    colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/xarm_* ${WS_SRC_DIR}/lite6_controller && \
     rm -rf ${WS_LOG_DIR}
 
+# Copy example svg images
+COPY ./svg svg
+
 ### Add workspace to the ROS entrypoint
 ### Source ROS workspace inside `~/.bashrc` to enable autocompletion
 RUN sed -i '$i source "${WS_INSTALL_DIR}/local_setup.bash" --' /ros_entrypoint.sh && \

README.md

@@ -1,47 +1,12 @@
 # drawing-robot-ros2
 
-## Building
+This repository contains ROS2 packages which make up a system used for drawing SVG images on different robots.
+These packages are in 'src/'.
 
-Requirements:
-- python3-pip 
-- python3-pil.imagetk
-- ros-humble-moveit
-- ros-humble-ros-gz
-- ignition-fortress
+Documentation and build scripts for the entire project are at the top level.
 
-``` sh
-./rebuild.sh
-```
-``` sh
-source src/install/local_setup.bash
-```
+The simplest way to run the project currently is by building and running the docker container.
 
-## Running
-Run
-``` sh
-ros2 run robot_controller dummy_controller
-```
-or
-``` sh
-ros2 launch axidraw_controller axidraw_controller
-```
-or
-``` sh
-ros2 launch lite6_controller lite6_gazebo.launch.py
-```
-or
-``` sh
-ros2 launch lite6_controller lite6_real.launch.py
-```
-or
-``` sh
-ros2 launch lite6_controller lite6_real_no_gui.launch.py
-```
-
-And simultaneously (using tmux or another terminal) run
-``` sh
-ros2 run drawing_controller drawing_controller src/draw_svg/svg/test.svg
-```
 ## Docker
 ### Build container
 
@@ -59,6 +24,64 @@ If active changes are being made, run:
 bash .docker/devel.bash
 ```
 This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
+
+## TODO Building locally
+
+Requirements:
+- python3-pip 
+- python3-pil.imagetk
+- ros-humble-moveit
+- ros-humble-ros-gz
+- ignition-fortress
+
+``` sh
+./rebuild.sh
+```
+``` sh
+source src/install/local_setup.bash
+```
+
+## Running
+### RobotController
+One of the following RobotControllers should be started:
+
+DummyController echoes Motion messages to the terminal.
+``` sh
+ros2 run robot_controller dummy_controller
+```
+
+AxidrawController draws on the axidraw robot
+``` sh
+ros2 launch axidraw_controller axidraw_controller
+```
+
+This starts the simulated lite6
+``` sh
+ros2 launch lite6_controller lite6_gazebo.launch.py
+```
+
+This runs the real lite6
+``` sh
+ros2 launch lite6_controller lite6_real.launch.py
+```
+
+This runs the real lite6 without Rviz (can be run on headless device over ssh)
+``` sh
+ros2 launch lite6_controller lite6_real_no_gui.launch.py
+```
+
+### DrawingController
+Once a RobotController is running, simultaneously (using tmux or another terminal) run
+``` sh
+ros2 run drawing_controller drawing_controller svg/test.svg
+```
+This will draw the svg image given as the last argument.
+
+## Creating compatible SVG images
+https://github.com/visioncortex/vtracer
+
+Use single layer (g) SVGs
+
 ## ROS2 rpi4
 https://github.com/ros-realtime/ros-realtime-rpi4-image/releases
 

convertlogtimestamp.sh

@@ -0,0 +1,24 @@
+#!/usr/bin/env sh
+# Converts ROS2 log data timestamps from stdin to more readable format
+
+
+while IFS= read -r string; do
+    timestr=`echo $string | egrep -o "\[[0-9]+\.[0-9]+\]"`
+    if [[ ! $timestr ]] then
+        continue
+    fi
+    timestamp=${timestr#"["}
+    timestamp=${timestamp%"]"}
+    sec=`echo $timestamp | cut -d "." -f 1`
+    nano=`echo $timestamp | cut -d "." -f 2`
+    #nanos=$(( $nano /  1000000000 ))
+    #sec=$(( $nanos + $sec ))
+    timestamp=`date -d @"$sec" +"%Y-%m-%d-%H:%M:%S"`
+    #printf 'Got sec:"%s"\n' "$sec"
+    #printf 'Got nano:"%s"\n' "$nano"
+    #printf 'Got timestamp:"%s"\n' "$timestamp"
+    #head=`echo $string | awk -F'$timestr' '{print $1}'`
+    #tail=`echo $string | awk -F'$timestr' '{print $2}'`
+    #printf '%s[%s]%s\n' "$head" "$timestamp" "$tail"
+    echo "${string/"$timestr"/"[${timestamp}.${nano}]"}"
+done

src/axidraw_controller/src/cpp/axidraw_controller.cpp

@@ -143,6 +143,8 @@ class AxidrawController : public RobotController
 
       auto p = points[i];
 
+      long unsigned int count = 0;
+
       // Ensure axidraw is not busy
       while (!is_ready())
       {
@@ -154,7 +156,13 @@ class AxidrawController : public RobotController
       if (p.z > 0)
         this->penup_pub->publish(std_msgs::msg::Empty());
       else
+      {
         this->pendown_pub->publish(std_msgs::msg::Empty());
+        while (i + count + 1 < points.size() && points[i + count + 1].z <= 0)
+        {
+          count++;
+        }
+      }
 
       // Wait for pen movement to complete
       while (!is_ready())
@@ -164,7 +172,18 @@ class AxidrawController : public RobotController
         loop_rate.sleep();
       }
 
+      if (count == 0)
         this->move_pub->publish(p);
+      else
+      {
+        std::vector<geometry_msgs::msg::Point> ps(&points[i],&points[i+count]);
+        robot_interfaces::msg::Points msg;
+        msg.points = ps;
+        std::string log = "Publishing path with " + std::to_string(ps.size()) + " points";
+        RCLCPP_INFO(this->get_logger(), log.c_str());
+        this->path_pub->publish(msg);
+      }
+      i += count;
 
       // Update status
       feedback->status = std::to_string(i+1) + "/" + std::to_string(points.size()) + " complete";

src/axidraw_controller/src/py/axidraw_serial.py

@@ -189,7 +189,7 @@ class AxidrawSerial(Node):
         '''
         self.set_busy()
 
-        self.get_logger().info("Received path: {}".format(msg))
+        self.get_logger().info("Received path: {}...".format(msg.points[:6]))
 
         path = [ [p.x,p.y] for p in msg.points ]
         self.ad.draw_path(path)

src/drawing_controller/drawing_controller/drawing_controller.py

@@ -84,7 +84,8 @@ class DrawingController(Node):
                 self.lines.append((p1,p2))
 
         self.svg_processor = SVGProcessor(self.get_logger())
-        print(self.svg_processor.process_svg(svgpath))
+        self.svg = self.svg_processor.process_svg(svgpath)
+        self.get_logger().info('Ready to begin executing motions')
 
     def send_goal(self, motion):
         self.busy = True
@@ -117,12 +118,13 @@ class DrawingController(Node):
         feedback = feedback_msg.feedback
         self.get_logger().info('Received feedback: {0}'.format(feedback))
 
-    def append_point(self, motion, point, height):
+    def append_points(self, motion, points):
+        for point in points:
             p = Pose()
             #self.get_logger().info('Appending point:{} {}'.format(point[0], point[1]))
-        p.position.x = point[0]
-        p.position.y = point[1]
-        p.position.z = height
+            p.position.x = float(point[0])
+            p.position.y = float(point[1])
+            p.position.z = float(point[2])
             q = quaternion_from_euler(0.0, math.pi, 0.0)
             p.orientation = Quaternion()
             p.orientation.x = q[0]
@@ -136,18 +138,14 @@ class DrawingController(Node):
     def timer_callback(self):
         if self.busy:
             return
-        next_line = self.lines[self.i]
+        if self.i >= len(self.svg):
+            self.get_logger().info('Finished executing all motions from SVG')
+            exit()
+        next_motion = self.svg[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, 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))
+        self.append_points(motion, next_motion)
+        self.i = self.i + 1
+        self.get_logger().info('Executing motion: {}...'.format(motion.path[:10]))
         self.send_goal(motion)
 
 

src/drawing_controller/drawing_controller/svg_processor.py

@@ -3,6 +3,7 @@
 import lxml.etree as ET
 import splipy.curve_factory as cf
 import numpy as np
+import math
 
 class SVGProcessor():
     """
@@ -35,6 +36,10 @@ class SVGProcessor():
             "polyline": self.primitive_polyline,
             "polygon": self.primitive_polygon,
         }
+        # Landscape A4: 210 x 297 mm
+        self.paper_width = 297
+        self.paper_height = 210
+
         self.map_point = self.map_point_function(1000,
                                                 1000)
 
@@ -47,7 +52,7 @@ class SVGProcessor():
             Returns:
                     primitive_fn ():
         '''
-        def log_error(p, _):
+        def log_error(p):
            self.logger.error("'{}' not supported".format(p.tag))
            return []
         return self.primitive_fns.get(primitive.tag, log_error)
@@ -104,19 +109,20 @@ class SVGProcessor():
         i = 0
         while i < len(pathstr):
             c = pathstr[i]
+            i += 1
             # 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 = c
+                while i < len(pathstr) and not (c.isspace() or c == ','):
+                    c = pathstr[i]
+                    if c != ',' 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)))
@@ -128,8 +134,19 @@ class SVGProcessor():
             nonlocal i
             i += 1
             return float(path[i])
+
+        def isfloat(element):
+            #If you expect None to be passed:
+            if element is None:
+                return False
+            try:
+                float(element)
+                return True
+            except ValueError:
+                return False
+
         def nextisnum():
-            return path[i+1].isdecimal()
+            return i + 1 < len(path) and isfloat(path[i + 1])
         def setpointup():
             nonlocal output
             p = self.map_point(x,y)
@@ -144,6 +161,7 @@ class SVGProcessor():
                 p = self.map_point(x,y)
                 output.append((p[0],p[1],0.0))
 
+
         while i < len(path):
             w = path[i]
             # MoveTo commands
@@ -176,27 +194,47 @@ class SVGProcessor():
                 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
+                    n = 50
                     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)
+                    coordinates = curve(lin)
+                    coordinates = np.nan_to_num(coordinates)
+                    #self.logger.info("Appending curve points: {}".format(coordinates))
+                    x = coordinates[-1][0]
+                    y = coordinates[-1][1]
                     appendpoints(coordinates)
                     if not nextisnum():
                         break
                 i += 1
                 continue
             if (w == "c"):
-                self.logger.error("SVG path parser '{}' not implemented".format(w))
+                while True:
+                    # https://github.com/sintef/Splipy/tree/master/examples
+                    control_points = [(x,y),
+                                      (x + getnum(), y + getnum()),
+                                      (x + getnum(), y + getnum()),
+                                      (x + getnum(), y + getnum())]
+                    control_points = np.array(control_points)
+                    n = 50
+                    curve = cf.cubic_curve(control_points)
+                    lin = np.linspace(curve.start(0), curve.end(0), n)
+                    coordinates = curve(lin)
+                    coordinates = np.nan_to_num(coordinates)
+                    #self.logger.info("Appending curve points: {}".format(coordinates))
+                    x = coordinates[-1][0]
+                    y = coordinates[-1][1]
+                    appendpoints(coordinates)
+                    if not nextisnum():
+                        break
+                i += 1
+                continue
             if (w == "S"):
                 self.logger.error("SVG path parser '{}' not implemented".format(w))
             if (w == "s"):
@@ -216,14 +254,15 @@ class SVGProcessor():
             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))
+            if (w == "Z" or w == "z"):
+               #TODO draw line if start and end point not are the same
+               i += 1
+               continue
+
             self.logger.error("SVG path parser panic mode at '{}'".format(w))
 
             i += 1
-        self.logger.info("Finished parsing path")
+        self.logger.info("Finished parsing path :'{}...' with {} points".format(output[:3], len(output)))
         return output
 
     # https://stackoverflow.com/questions/30232031/how-can-i-strip-namespaces-out-of-an-lxml-tree
@@ -242,35 +281,83 @@ class SVGProcessor():
             svg = xml.getroot()
             svg = self.strip_ns_prefix(svg)
 
-            if 'width' in svg.attrib:
+            if 'viewBox' in svg.attrib:
+                vb = svg.get('viewBox').split(' ')
+                self.map_point = self.map_point_function(float(vb[2]),
+                                                         float(vb[3]))
+                self.logger.info("Got width:{} and height:{} from viewBox".format(vb[2],vb[3]))
+            elif '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
+                self.logger.info("Got width:{} and height:{} from width and height attributes".format(svg.get('width'),svg.get('height')))
             else:
                self.logger.error("Unable to get SVG dimensions")
 
             motions = []
+            def process_tags(svg):
+                nonlocal motions
                 for child in svg:
-               self.logger.info("Attempting to process SVG primitive:'{}'".format(child.tag))
+                    self.logger.debug("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)
+                        #for m in self.path_parser(child):
+                        #    motions.append(m)
+                        motions.append(self.path_parser(child))
+                    elif (child.tag == 'g'):
+                        self.logger.info("Recursively processing SVG primitive:'{}'".format(child.tag))
+                        process_tags(child)
                     else:
                         primitive_fn = self.get_primitive_fn(child)
                         motions.append(primitive_fn(child))
 
+            process_tags(svg)
+
             motions_refined = []
             for m in motions:
                 if m == []:
                     continue
-                self.logger.info("Refining:'{}'".format(m))
-                motions_refined.append(self.down_and_up(m))
+
+                mm = self.remove_homes(m)
+                mm = self.remove_redundant(mm)
+
+                #self.logger.info("Refining:'{}...'".format(m[:3]))
+                motions_refined.append(self.down_and_up(mm))
+
+            # Move to home at end
+            motions_refined.append([(0.0, 0.0, 1.0)])
             return motions_refined
 
+    def remove_homes(self, motion):
+        # Remove unnecessary moves home
+        mm = []
+        for p in motion:
+            if p[0] <= 0.0 and p[1] <= 0.0:
+                continue
+            mm.append(p)
+        return mm
+
+    def remove_redundant(self, motion):
+        # Remove points that are too close to the previous point, specified by the tolerance
+        mm = []
+        tolerance = 0.001
+        prev = (-1, -1, 0)
+        for i, p in enumerate(motion):
+            x = p[0]
+            y = p[1]
+            z = p[2]
+            px = prev[0]
+            py = prev[1]
+            pz = prev[2]
+            xdiff = abs(x - px)
+            ydiff = abs(y - py)
+            zdiff = abs(z - pz)
+            if xdiff < tolerance and ydiff < tolerance and zdiff < tolerance:
+                continue
+            prev = p
+            mm.append(p)
+        return mm
+
     def translate(self, val, lmin, lmax, rmin, rmax):
         lspan = lmax - lmin
         rspan = rmax - rmin
@@ -278,9 +365,18 @@ class SVGProcessor():
         return rmin + (val * rspan)
 
     def map_point_function(self, x_pixels, y_pixels):
+        x_offset = 0
+        y_offset = 0
+        if (x_pixels > y_pixels):
+            ratio =  self.paper_height / self.paper_width
+            y_offset = x_pixels * ratio - y_pixels
+        else:
+            ratio = self.paper_width / self.paper_height
+            x_offset = y_pixels * ratio - x_pixels
+
         def map_point(xpix,ypix):
-            x = self.translate(xpix, 0, x_pixels, 0, 1)
-            y = self.translate(ypix, 0, y_pixels, 0, 1)
+            x = self.translate(xpix, 0, x_pixels + x_offset, 0, 1)
+            y = self.translate(ypix, 0, y_pixels + y_offset, 0, 1)
             return (x,y)
         return map_point
 

src/lite6_controller/launch/lite6_gazebo.launch.py

@@ -304,6 +304,12 @@ def launch_setup(context, *args, **kwargs):
         joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
         joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 0.5
 
+    kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
+    #kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin'
+    #kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
+    kinematics_yaml['kinematics_solver_timeout'] = 10.0
+    kinematics_yaml['kinematics_solver_attempts'] = 10
+
     #print(joint_limits_yaml)
     #quit()
 
@@ -369,12 +375,18 @@ def launch_setup(context, *args, **kwargs):
     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""",
+            # Disable AddTimeOptimalParameterization to fix motion blending https://github.com/ros-planning/moveit/issues/2905
+            'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
+            #'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
             "default_planner_config": "PTP",
-            "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
         }
     }
 
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
+
 
     # Moveit controllers Configuration
     moveit_controllers = {
@@ -416,10 +428,12 @@ def launch_setup(context, *args, **kwargs):
         package='moveit_ros_move_group',
         executable='move_group',
         output='screen',
+        arguments=['--log-level', 'debug'],
         parameters=[
             robot_description_parameters,
             #ompl_planning_pipeline_config,
             pilz_planning_pipeline_config,
+            move_group_capabilities,
             trajectory_execution,
             plan_execution,
             moveit_controllers,

src/lite6_controller/launch/lite6_real.launch.py

@@ -153,6 +153,14 @@ def launch_setup(context, *args, **kwargs):
     controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
     ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
     kinematics_yaml = robot_description_parameters['robot_description_kinematics']
+
+    #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)
 
     if add_gripper.perform(context) in ('True', 'true'):
@@ -171,6 +179,11 @@ def launch_setup(context, *args, **kwargs):
         if joint_limits_yaml and gripper_joint_limits_yaml:
             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'] = 0.5
+
     add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
     add_prefix_to_moveit_params(
         controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml,
@@ -222,6 +235,20 @@ def launch_setup(context, *args, **kwargs):
         # },
     }
 
+
+    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""",
+            '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",
+        }
+    }
+
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
     # Start the actual move_group node/action server
     move_group_node = Node(
         package='moveit_ros_move_group',
@@ -229,7 +256,9 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[
             robot_description_parameters,
-            ompl_planning_pipeline_config,
+            #ompl_planning_pipeline_config,
+            pilz_planning_pipeline_config,
+            move_group_capabilities,
             trajectory_execution,
             plan_execution,
             moveit_controllers,

src/lite6_controller/launch/lite6_real_no_gui.launch.py

@@ -196,12 +196,16 @@ def launch_setup(context, *args, **kwargs):
     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""",
+            #'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",
-            "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
         }
     }
 
+    move_group_capabilities = {
+        "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
+    }
+
     # Moveit controllers Configuration
     moveit_controllers = {
         moveit_controller_manager_key.perform(context): controllers_yaml,
@@ -246,6 +250,7 @@ def launch_setup(context, *args, **kwargs):
             robot_description_parameters,
             #ompl_planning_pipeline_config,
             pilz_planning_pipeline_config,
+            move_group_capabilities,
             trajectory_execution,
             plan_execution,
             moveit_controllers,

src/lite6_controller/lite6_controller.repos

@@ -1,9 +1,5 @@
 repositories:
-  ros2_controllers:
-    type: git
-    url: https://github.com/AndrejOrsula/ros2_controllers.git
-    version: jtc_effort
   xarm_ros2:
     type: git
     url: https://github.com/xArm-Developer/xarm_ros2
-    version: galactic
+    version: humble

src/lite6_controller/src/lite6_controller.cpp

@@ -2,6 +2,10 @@
 #include <rclcpp/rclcpp.hpp>
 #include <robot_controller/robot_controller.hpp>
 #include <chrono>
+//#include <queue>
+
+#include <fstream>
+#include <iostream>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
@@ -26,6 +30,7 @@
 #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>
@@ -67,6 +72,18 @@ public:
   moveit_cpp::MoveItCppPtr moveit_cpp_;
   moveit_cpp::PlanningComponentPtr planning_component_;
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
+  rclcpp::Client<moveit_msgs::srv::GetMotionSequence>::SharedPtr sequence_client_;
+
+  //std::queue<std::pair<moveit_msgs::msg::RobotTrajectory,rclcpp_action::ServerGoalHandle<ExecuteMotion>>> trajectory_queue;
+
+  // Set limits for A4 paper
+  // 297x210
+  float xlim_lower = 0.10;
+  float xlim_upper = 0.30;
+  float ylim_lower = -0.14;
+  float ylim_upper = 0.14;
+  float zlim_lower = 0.19;
+  float zlim_upper = 0.21;
 
   //bool moved = false;
   //
@@ -77,7 +94,7 @@ public:
   /**
    * CommandListManager, used to plan MotionSequenceRequest
    */
-  pilz_industrial_motion_planner::CommandListManager command_list_manager;
+  //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());
 
   /**
@@ -85,10 +102,10 @@ public:
    */
   Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
     : RobotController(options),
-      move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP),
+      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(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());
@@ -105,7 +122,8 @@ public:
   void setup()
   {
     //moveit_cpp_ = std::make_shared<moveit_cpp::MoveItCpp>(move_group.getNodeHandle());
-    try {
+    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_);
@@ -113,6 +131,17 @@ public:
 
       planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(this->shared_from_this(),
                                                                                                "robot_description");
+      this->sequence_client_ =
+        this->create_client<moveit_msgs::srv::GetMotionSequence>("plan_sequence_path");
+
+      while (!sequence_client_->wait_for_service(1s)) {
+        if (!rclcpp::ok()) {
+          RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the 'plan_sequence_path' service. Exiting.");
+          return;
+        }
+        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "'plan_sequence_path' service not available, waiting again...");
+      }
+
     }
     catch (int e) {
       RCLCPP_ERROR(this->get_logger(), "Initialization Failed: %d", e);
@@ -125,13 +154,60 @@ public:
 
   }
 
+
   /**
-   * This function takes a pose and returns a MotionPlanRequest
+   * Function that translates an input value with a given range to a value within another range.
    */
-  planning_interface::MotionPlanRequest createRequest(geometry_msgs::msg::PoseStamped pose)
+  float translate(float val, float lmin, float lmax, float rmin, float rmax)
   {
+    float lspan = lmax - lmin;
+    float rspan = rmax - rmin;
+    float out = (val - lmin) / lspan;
+    out = rmin + (val * rspan);
+
+    // Ensure that output is within bounds
+    out = std::max(rmin, out);
+    out = std::min(rmax, out);
+    return out;
+  }
+
+  /**
+   * Translates a pose to the xArm coordinate frame
+   */
+  geometry_msgs::msg::PoseStamped translatePose(geometry_msgs::msg::PoseStamped pose)
+  {
+    // TODO support paper angle
+    auto x = pose.pose.position.x;
+    auto y = pose.pose.position.y;
+    // X and Y are swapped in xArm space
+    pose.pose.position.x = translate(y, 0, 1, xlim_lower, xlim_upper);
+    pose.pose.position.y = translate(x, 0, 1, ylim_lower, ylim_upper);
+    pose.pose.position.z = translate(pose.pose.position.z, 0, 1, zlim_lower, zlim_upper);
+
+    return pose;
+  }
+
+  /**
+   *
+   */
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  {
+    moveit_msgs::msg::MotionSequenceRequest msr = moveit_msgs::msg::MotionSequenceRequest();
+    //waypoints.push_back(move_group.getCurrentPose().pose);
+    std::string ee_link = move_group.getLinkNames().back();
+    RCLCPP_INFO(this->get_logger(), "Got ee_link: %s", ee_link.c_str());
+    for (auto p : *path)
+    {
+      //RCLCPP_INFO(this->get_logger(), "Creating MSI");
+      moveit_msgs::msg::MotionSequenceItem msi = moveit_msgs::msg::MotionSequenceItem();
+
       planning_interface::MotionPlanRequest mpr = planning_interface::MotionPlanRequest();
       mpr.planner_id = "PTP";
+      mpr.group_name = move_group.getName();
+      mpr.max_velocity_scaling_factor = 0.5;
+      mpr.max_acceleration_scaling_factor = 0.5;
+      mpr.allowed_planning_time = 10;
+      mpr.max_cartesian_speed = 1; // m/s
       //mpr.goal_constraints.position_constraints.header.frame_id = "world";
 
       // A tolerance of 0.01 m is specified in position
@@ -144,104 +220,134 @@ public:
       //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);
+
+      //moveit_msgs::msg::Constraints pose_goal =
+      //  kinematic_constraints::constructGoalConstraints(ee_link, p, tolerance_pose, tolerance_angle);
       //kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle);
 
+      geometry_msgs::msg::PointStamped point;
+      auto position = translatePose(p).pose.position;
+      point.point = position;
+      moveit_msgs::msg::Constraints pose_goal =
+      //kinematic_constraints::constructGoalConstraints(ee_link, point, 1e-5);
+      kinematic_constraints::constructGoalConstraints(ee_link, translatePose(p), 1e-3, 1e-2);
+      //kinematic_constraints::constructGoalConstraints(ee_link, p, 1.0, 1.0);
+      //kinematic_constraints::constructGoalConstraints(ee_link, p, 1e-3, 1e-2);
+
       mpr.goal_constraints.push_back(pose_goal);
-    return mpr;
+
+      msi.req = mpr;
+      msi.blend_radius = 0.0; //TODO make configurable
+      //msi.blend_radius = 0.000000001; //TODO make configurable
+      msr.items.push_back(msi);
     }
-
-  // TODO implement time param
-  // https://moveit.picknik.ai/foxy/doc/move_group_interface/move_group_interface_tutorial.html
-  // https://groups.google.com/g/moveit-users/c/MOoFxy2exT4
-  // https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotTrajectory.html
-
-  // 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
-  //
-
-  // 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);
+    msr.items.back().blend_radius = 0.0; // Last element blend must be 0
+    moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState();
+    moveit_msgs::msg::RobotState state_msg;
+    moveit::core::robotStateToRobotStateMsg(*move_group_state, state_msg);
+    msr.items.front().req.start_state = state_msg;
+    return msr;
   }
 
   /**
-   * Translates a pose to the xArm coordinate frame
+   *
    */
-  geometry_msgs::msg::PoseStamped translatePose(geometry_msgs::msg::PoseStamped pose)
+  moveit_msgs::msg::MotionSequenceRequest createMotionSequenceRequest(const std::vector<geometry_msgs::msg::PoseStamped> *path, double x_offset, double y_offset, double z_offset)
   {
-    // 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);
+    this->xlim_lower += x_offset;
+    this->xlim_upper += x_offset;
+    this->ylim_lower += y_offset;
+    this->ylim_upper += y_offset;
+    this->zlim_lower += z_offset;
+    this->zlim_upper += z_offset;
+    moveit_msgs::msg::MotionSequenceRequest msr = createMotionSequenceRequest(path);
+    this->xlim_lower -= x_offset;
+    this->xlim_upper -= x_offset;
+    this->ylim_lower -= y_offset;
+    this->ylim_upper -= y_offset;
+    this->zlim_lower -= z_offset;
+    this->zlim_upper -= z_offset;
+    return msr;
   }
 
   /**
-   * 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)
+  static void appendLineToFile(std::string filepath, std::string line)
   {
-    pose = translatePose(pose);
-    move_group.setPoseTarget(pose);
-    //moveit_msgs::msg::RobotTrajectory trajectory;
-    //move_group.setPlanningPipelineId("PTP");
-    move_group.setPlannerId("PTP");
+    std::ofstream file;
+    file.open(filepath, std::ios::out | std::ios::app);
+    if (file.fail())
+        throw std::ios_base::failure(std::strerror(errno));
 
-    robot_trajectory::RobotTrajectory previous_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
-    previous_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), *trajectory);
+    file.exceptions(file.exceptions() | std::ios::failbit | std::ifstream::badbit);
 
+    file << line << std::endl;
+  }
 
-    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");
+  /**
+   *
+   */
+  std::string pointsToString(const std::vector<geometry_msgs::msg::PoseStamped> *path, double x_offset, double y_offset, double z_offset)
+  {
+    std::string out = "";
+    this->xlim_lower += x_offset;
+    this->xlim_upper += x_offset;
+    this->ylim_lower += y_offset;
+    this->ylim_upper += y_offset;
+    this->zlim_lower += z_offset;
+    this->zlim_upper += z_offset;
+    for (auto p : *path)
+    {
+      auto position = translatePose(p).pose.position;
+      out = out + std::to_string(position.x) + " " +
+                  std::to_string(position.y) + " " +
+                  std::to_string(position.z) + ",";
+    }
+    out = out.substr(0, out.size()-1); //remove trailing comma
+    this->xlim_lower -= x_offset;
+    this->xlim_upper -= x_offset;
+    this->ylim_lower -= y_offset;
+    this->ylim_upper -= y_offset;
+    this->zlim_lower -= z_offset;
+    this->zlim_upper -= z_offset;
+    return out;
+  }
 
-    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();
+  /**
+   * Tests path with offsets in multiple locations and logs them to OUTPUT.csv
+   */
+  void planAndLogOffset(const std::vector<geometry_msgs::msg::PoseStamped> *path)
+  {
+    double res = 0.05;
+    double bot = -0.5;
+    double top = 0.5;
+    for (double x = bot; x <= top; x+=res)
+    {
+      for (double y = bot; y <= top; y+=res)
+      {
+        for (double z = bot; z <= top; z+=res)
+        {
+          auto msr = createMotionSequenceRequest(path, x, y, z);
+          auto req = std::make_shared<moveit_msgs::srv::GetMotionSequence::Request>();
+          req->request = msr;
+          auto res = sequence_client_->async_send_request(req);
+          auto ts = res.get()->response.planned_trajectories;
+          //RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
+          std::string status = "";
+          if (ts.size() > 0)
+          {
+            status = "success";
+          }
+          else
+          {
+            status = "failure";
+          }
+          status = status + "," + pointsToString(path,x,y,z);
+          appendLineToFile("OUTPUT.csv", status);
+        }
+      }
+    }
   }
 
   /**
@@ -256,86 +362,47 @@ public:
     auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
     auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
 
-    // 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);
+    auto msr = createMotionSequenceRequest(&goal->motion.path);
+    RCLCPP_INFO(this->get_logger(), "Created MSR");
 
-    //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");
+    //planAndLogOffset(&goal->motion.path);
 
-    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)
+    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)
     {
-      //RCLCPP_INFO(this->get_logger(), "Planning trajectory");
+      status = "success";
+      RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
+      RCLCPP_INFO(this->get_logger(), "Executing result");
+      move_group.execute(ts[0]);
 
-      // Append next pose to trajectory
-      addPoseToTrajectory(p, &multi_trajectory);
+      status = status + "," + pointsToString(&goal->motion.path,0,0,0);
+      //appendLineToFile("OUTPUT.csv", status);
 
-      // 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();
-    }
-
-    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);
-
-
-    //waypoints.clear();
-
-    //for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) {
-    //  // Check if there is a cancel request
-    //  if (goal_handle->is_canceling()) {
-    //    result->result = feedback->status;
-    //    goal_handle->canceled(result);
-    //    RCLCPP_INFO(this->get_logger(), "Goal canceled");
-    //    return;
-    //  }
-    //  // Update status
-    //  feedback->status = std::to_string(i) + "/10 complete";
-    //  // Publish feedback
-    //  goal_handle->publish_feedback(feedback);
-    //  RCLCPP_INFO(this->get_logger(), feedback->status.c_str());
-
-    //  loop_rate.sleep();
-    //}
-
-    // Check if goal is done
-    if (rclcpp::ok()) {
       result->result = "success";
       goal_handle->succeed(result);
       RCLCPP_INFO(this->get_logger(), "Goal succeeded");
+      return;
     }
+
+    status = "failure";
+    status = status + "," + pointsToString(&goal->motion.path,0,0,0);
+    //appendLineToFile("OUTPUT.csv", status);
+
+    RCLCPP_ERROR(this->get_logger(), "Planner failed to return trajectory in time");
+    result->result = "failure";
+    goal_handle->succeed(result);
+    // abort prevents action from being called
+    //goal_handle->abort(result);
+
+    RCLCPP_ERROR(this->get_logger(), "Goal failed");
   }
 };
 

src/robot_controller/include/robot_controller/robot_controller.hpp

@@ -58,7 +58,7 @@ rclcpp_action::GoalResponse RobotController::motion_handle_goal(
   const rclcpp_action::GoalUUID & uuid,
   std::shared_ptr<const ExecuteMotion::Goal> goal)
 {
-  RCLCPP_INFO(this->get_logger(), "Received goal request with acceleration %f", goal->motion.acceleration);
+  RCLCPP_INFO(this->get_logger(), "Received goal request with %ld poses", goal->motion.path.size());
   (void)uuid;
   return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 }