diff --git a/README.md b/README.md
index 27fd91d..1462a6d 100644
--- a/README.md
+++ b/README.md
@@ -1 +1,11 @@
-# drawing-robot-ros2
\ No newline at end of file
+# 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
+```
diff --git a/src/draw_svg/package.xml b/src/draw_svg/package.xml
index 9a7f99a..8822291 100644
--- a/src/draw_svg/package.xml
+++ b/src/draw_svg/package.xml
@@ -13,6 +13,7 @@
rclcpp
geometry_msgs
tf2_ros
+ python-lxml
ament_lint_auto
diff --git a/src/draw_svg/rebuild.sh b/src/draw_svg/rebuild.sh
index f0b2fc4..02bb610 100644
--- a/src/draw_svg/rebuild.sh
+++ b/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
diff --git a/src/draw_svg/src/py/draw_svg.py b/src/draw_svg/src/py/draw_svg.py
index b226770..08d0398 100755
--- a/src/draw_svg/src/py/draw_svg.py
+++ b/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)
diff --git a/src/draw_svg/src/py/readsvg.py b/src/draw_svg/src/py/readsvg.py
new file mode 100644
index 0000000..39a221d
--- /dev/null
+++ b/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'))
diff --git a/src/draw_svg/svg/test.svg b/src/draw_svg/svg/test.svg
new file mode 100644
index 0000000..ab36963
--- /dev/null
+++ b/src/draw_svg/svg/test.svg
@@ -0,0 +1,5 @@
+