Implement svg file drawing

README.md

@@ -1 +1,11 @@
-# drawing-robot-ros2
+# drawing-robot-ros2
+
+
+``` sh
+source install/local_setup.bash
+cd src/ign_moveit2_examples/src/draw_svg/
+rosdep install --from-paths . --ignore-src -r -y
+colcon build
+source install/local_setup.bash
+ros2 launch draw_svg draw_svg.launch.py
+```

src/draw_svg/package.xml

@@ -13,6 +13,7 @@
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>python-lxml</depend>
   <!-- <depend>moveit_ros_planning_interface</depend> -->
 
   <test_depend>ament_lint_auto</test_depend>

src/draw_svg/rebuild.sh

@@ -1,3 +1,4 @@
 rm -r build/ install/ log/
+rosdep install --from-paths . --ignore-src -r -y
 colcon build
 #ros2 launch draw_svg draw_svg.launch.py

src/draw_svg/src/py/draw_svg.py

@@ -11,6 +11,8 @@ from rclpy.qos import QoSProfile
 from random import uniform as rand
 import math
 #from tf2_ros.transformations import quaternion_from_euler
+import lxml.etree as ET
+
 
 def quaternion_from_euler(ai, aj, ak):
     ai /= 2.0
@@ -35,6 +37,19 @@ def quaternion_from_euler(ai, aj, ak):
 
     return q
 
+def translate(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(x_pixels, y_pixels, xlim_lower, xlim_upper, ylim_lower, ylim_upper):
+    def map_point(xpix,ypix):
+       x = translate(xpix, 0, x_pixels, xlim_lower, xlim_upper)
+       y = translate(ypix, 0, y_pixels, ylim_lower, ylim_upper)
+       return (x,y)
+    return map_point
+
 
 class PublishTarget(Node):
     def __init__(self):
@@ -44,14 +59,27 @@ class PublishTarget(Node):
         self.timer = self.create_timer(timer_period, self.timer_callback)
         self.i = 0
 
+        # TODO get dimensions from svg
+
         #print(p)
         #print(p.position)
         #print(p.orientation)
+        xml = ET.parse('svg/test.svg')
+        svg = xml.getroot()
+        self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.0, 0.5, 0.3, 0.8)
+        self.points = []
+        for child in svg:
+            if (child.tag == 'line'):
+                self.points.append((float(child.get('x1')), float(child.get('y1'))))
+                self.points.append((float(child.get('x2')), float(child.get('y2'))))
+
 
     def timer_callback(self):
+        next_point = self.points[self.i]
+        point = self.map_point(float(next_point[0]),float(next_point[1]))
         p = Pose()
-        p.position.x = rand(0.1,0.4)
-        p.position.y = rand(0.1,0.4)
+        p.position.x = point[0]
+        p.position.y = point[1]
         p.position.z = 0.1
         q = quaternion_from_euler(0.0, math.pi, 0.0)
         #p.orientation = q
@@ -64,7 +92,7 @@ class PublishTarget(Node):
         #print(ps)
         self.publisher_.publish(ps)
         self.get_logger().info('Publishing to /target_pose: "%s"' % p)
-        self.i += 1
+        self.i = (self.i + 1) % len(self.points)
 
 def main(args=None):
     rclpy.init(args=args)

src/draw_svg/src/py/readsvg.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+
+import lxml.etree as ET
+
+xml = ET.parse('svg/test.svg')
+svg = xml.getroot()
+print(svg)
+
+# AttributeError: 'lxml.etree._ElementTree' object has no attribute 'tag'
+print(svg.tag)
+
+# TypeError: 'lxml.etree._ElementTree' object is not subscriptable
+print(svg[0])
+
+# TypeError: 'lxml.etree._ElementTree' object is not iterable
+for child in svg:
+    print('width:', svg.get('width'))
+    if (child.tag == 'line'):
+        print(child.get('x1'))
+        print(child.get('x2'))
+        print(child.get('y1'))
+        print(child.get('y2'))

src/draw_svg/svg/test.svg

@@ -0,0 +1,5 @@
+<svg height="210" width="500">
+  <line x1="0" y1="0" x2="200" y2="200" style="stroke:rgb(255,0,0);stroke-width:2" />
+  <line x1="0" y1="200" x2="200" y2="0" style="stroke:rgb(255,0,0);stroke-width:2" />
+  <line x1="200" y1="0" x2="200" y2="200" style="stroke:rgb(255,0,0);stroke-width:2" />
+</svg>