From 14e371cae071d14cadec6258f7d3833a241cb161 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Mon, 30 Jan 2023 14:17:55 +0200
Subject: [PATCH] Implement initial SVGProcessor

---
 .../drawing_controller/drawing_controller.py  |  6 +++
 .../drawing_controller/svg_processor.py       | 41 +++++++++++++++++++
 2 files changed, 47 insertions(+)
 create mode 100644 src/drawing_controller/drawing_controller/svg_processor.py

diff --git a/src/drawing_controller/drawing_controller/drawing_controller.py b/src/drawing_controller/drawing_controller/drawing_controller.py
index 25a38ff..3bd5e03 100644
--- a/src/drawing_controller/drawing_controller/drawing_controller.py
+++ b/src/drawing_controller/drawing_controller/drawing_controller.py
@@ -17,6 +17,8 @@ from robot_interfaces.msg import Motion
 import sys
 from copy import deepcopy
 
+from drawing_controller.svg_processor import SVGProcessor
+
 def quaternion_from_euler(ai, aj, ak):
     ai /= 2.0
     aj /= 2.0
@@ -55,6 +57,7 @@ def map_point_function(x_pixels, y_pixels, xlim_lower, xlim_upper, ylim_lower, y
 
 
 class DrawingController(Node):
+
     def __init__(self, svgpath):
         super().__init__('drawing_controller')
         #self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10)
@@ -76,6 +79,9 @@ class DrawingController(Node):
                 p2 = (float(child.get('x2')), float(child.get('y2')))
                 self.lines.append((p1,p2))
 
+        self.svg_processor = SVGProcessor(self.get_logger())
+        self.svg_processor.process_svg(svgpath)
+
     def send_goal(self, motion):
         self.busy = True
         goal_msg = ExecuteMotion.Goal()
diff --git a/src/drawing_controller/drawing_controller/svg_processor.py b/src/drawing_controller/drawing_controller/svg_processor.py
new file mode 100644
index 0000000..941feb2
--- /dev/null
+++ b/src/drawing_controller/drawing_controller/svg_processor.py
@@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+
+import rclpy
+import lxml.etree as ET
+
+class SVGProcessor():
+
+    def __init__(self, logger):
+        self.logger = logger
+        # A dict containing svg primitive names mapping to functions that handle them
+        self.primitives = {
+            # Reference:
+            # https://developer.mozilla.org/en-US/docs/Web/SVG/Element
+            #"line": lambda p: print("LINE"),
+        }
+
+    def get_primitive(self, primitive):
+       log_error = lambda p: self.logger.error("'{}' not supported".format(p.tag))
+       return self.primitives.get(primitive.tag, log_error)
+
+    def process_svg(self, svg_path):
+        with open(svg_path) as svg:
+            xml = ET.parse(svg)
+            svg = xml.getroot()
+
+            for child in svg:
+               f = self.get_primitive(child)
+               f(child)
+
+    def translate(self, val, lmin, lmax, rmin, rmax):
+        lspan = lmax - lmin
+        rspan = rmax - rmin
+        val = float(val - lmin) / float(lspan)
+        return rmin + (val * rspan)
+
+    def map_point_function(self, x_pixels, y_pixels):
+        def map_point(xpix,ypix):
+            x = self.translate(xpix, 0, x_pixels, 0, 1)
+            y = self.translate(ypix, 0, y_pixels, 0, 1)
+            return (x,y)
+        return map_point