Add path simplification function

Dockerfile

@@ -25,19 +25,20 @@ RUN apt-get update && \
     apt-get install -yq python3-pil.imagetk && \
     apt-get install -yq ros-${ROS_DISTRO}-pilz-industrial-motion-planner && \
     apt-get install -yq tmux && \
+    apt-get install -yq python3-pip && \
     apt-get install -yq ros-${ROS_DISTRO}-desktop && \
     apt-get install -yq ros-${ROS_DISTRO}-rclcpp-components
 
 ### Install AxiDraw
-RUN apt-get update && \
+#RUN apt-get update && \
-    apt-get install -yq python3-pip && \
+#    apt-get install -yq python3-pip && \
-    pip install --upgrade --upgrade-strategy eager packaging && \
+#    pip install --upgrade --upgrade-strategy eager packaging && \
-    pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
+#    pip install https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip --upgrade --upgrade-strategy eager
 
 ### Install splipy
-RUN apt-get update && \
+#RUN apt-get update && \
-    apt-get install -yq python3-pip && \
+#    apt-get install -yq python3-pip && \
-    pip install --upgrade --upgrade-strategy eager splipy
+#    pip install --upgrade --upgrade-strategy eager splipy
 
 # Build interfaces and generic controller first
 COPY ./src/robot_interfaces ${WS_SRC_DIR}/robot_interfaces
@@ -50,6 +51,8 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
 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 pip install -r ${WS_SRC_DIR}/drawing_controller/requirements.txt
+RUN pip install -r ${WS_SRC_DIR}/axidraw_controller/requirements.txt
 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 && \

src/axidraw_controller/requirements.txt

@@ -0,0 +1 @@
+axicli @ https://cdn.evilmadscientist.com/dl/ad/public/AxiDraw_API.zip

src/drawing_controller/drawing_controller/svg_processor.py

@@ -4,6 +4,7 @@ import lxml.etree as ET
 import splipy.curve_factory as cf
 import numpy as np
 import math
+import simplification.cutil
 
 class SVGProcessor():
     """
@@ -117,8 +118,8 @@ class SVGProcessor():
             # Numbers
             if c == '+' or c == '-' or c.isdecimal():
                 s = c
-                isdelim = lambda x: x.isspace() or x == ',' or x.isalpha() or x == '+'
+                isdelim = lambda x: x.isspace() or x.isalpha() or x in [',', '+']
-                while i < len(pathstr) and (not (isdelim(c) or c == '-')):
+                while i < len(pathstr) and not isdelim(c):
                     c = pathstr[i]
                     if not isdelim(c):
                         s = s + c
@@ -139,7 +140,6 @@ class SVGProcessor():
             return float(path[i])
 
         def isfloat(element):
-            #If you expect None to be passed:
             if element is None:
                 return False
             try:
@@ -255,13 +255,15 @@ class SVGProcessor():
                     control_points = np.array(control_points)
                     maxval = np.amax(np.absolute(control_points))
                     control_points = control_points / maxval #normalize values
-                    n = 50
+                    n = 500
                     curve = cf.cubic_curve(control_points)
                     lin = np.linspace(curve.start(0), curve.end(0), n)
                     coordinates = curve(lin)
                     coordinates = np.nan_to_num(coordinates)
                     coordinates = coordinates * maxval #denormalize values
                     #self.logger.info("Appending curve points: {}".format(coordinates))
+                    #print(coordinates)
+                    #input()
                     x = coordinates[-1][0]
                     y = coordinates[-1][1]
                     appendpoints(coordinates)
@@ -382,6 +384,10 @@ class SVGProcessor():
 
                 mm = self.remove_homes(m)
                 mm = self.remove_redundant(mm)
+                #print('before:', len(mm))
+                mm = self.simplify(mm)
+                #print('after:', len(mm))
+                #input()
 
                 #self.logger.info("Refining:'{}...'".format(m[:3]))
                 motions_refined.append(self.down_and_up(mm))
@@ -420,6 +426,33 @@ class SVGProcessor():
             mm.append(p)
         return mm
 
+    def simplify(self, motion):
+        """
+        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.0001
+
+        tmp = []
+        out = []
+        lastup = True
+        sf = lambda l: [ (p[0],p[1],0.0) for p in simplification.cutil.simplify_coords(l, epsilon) ]
+        for p in motion:
+            penup = p[2] > 0
+            if penup and not lastup:
+                out += sf(tmp)
+                tmp = []
+            if penup:
+                out.append(p)
+            else:
+                tmp.append(list(p)[:-1])
+            lastup = penup
+
+        if (len(tmp) > 0):
+            out += sf(tmp)
+
+        return out
+
     def translate(self, val, lmin, lmax, rmin, rmax):
         lspan = lmax - lmin
         rspan = rmax - rmin

src/drawing_controller/requirements.txt

@@ -0,0 +1,4 @@
+lxml==4.9.2
+numpy==1.24.2
+Splipy==1.6.0
+simplification==0.6.2