Fix unnecessary axidraw pen lift

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):
@@ -63,6 +64,7 @@ class AxidrawSerial(Node):
         self.ad.update()                     # Process changes to options
         self.status["serial"] = "ready"
         self.status["motion"] = "ready"
+        self.status["pen"] = "up"
         return True
 
     def __init__(self):
@@ -166,7 +168,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received penup: {}".format(msg))
 
+        if self.status['pen'] == "down":
             self.ad.penup()
+            self.status['pen'] = "up"
         self.set_ready()
 
     def pendown_callback(self, msg):
@@ -179,7 +183,9 @@ class AxidrawSerial(Node):
 
         self.get_logger().info("Received pendown: {}".format(msg))
 
+        if self.status['pen'] == "up":
             self.ad.pendown()
+            self.status['pen'] = "down"
         self.set_ready()
 
     def stroke_callback(self, msg):
@@ -194,6 +200,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/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]
@@ -215,6 +215,7 @@ class SVGProcessor():
                 tmp.append(list(p)[:-1])
             lastup = penup
 
+        # Handle anything left in tmp
         if (len(tmp) > 0):
             out += sf(tmp)