From cb1923e56abd6cd28f4cd378c3b9954d050b3798 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Fri, 31 Mar 2023 12:13:35 +0300
Subject: [PATCH] Fix unnecessary axidraw pen lift

---
 src/axidraw_controller/src/cpp/axidraw_controller.cpp |  2 +-
 src/axidraw_controller/src/py/axidraw_serial.py       | 11 +++++++++--
 .../drawing_controller/svg_processor.py               |  3 ++-
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/src/axidraw_controller/src/cpp/axidraw_controller.cpp b/src/axidraw_controller/src/cpp/axidraw_controller.cpp
index 3af64a2..f598b99 100644
--- a/src/axidraw_controller/src/cpp/axidraw_controller.cpp
+++ b/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++;
diff --git a/src/axidraw_controller/src/py/axidraw_serial.py b/src/axidraw_controller/src/py/axidraw_serial.py
index c4e4d69..b01f173 100755
--- a/src/axidraw_controller/src/py/axidraw_serial.py
+++ b/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))
 
-        self.ad.penup()
+        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))
 
-        self.ad.pendown()
+        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()
 
 
diff --git a/src/drawing_controller/drawing_controller/svg_processor.py b/src/drawing_controller/drawing_controller/svg_processor.py
index dfd2c56..56d22ed 100644
--- a/src/drawing_controller/drawing_controller/svg_processor.py
+++ b/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)