From cb7c7975dc06e2b51db5e125bc377521a7a0b01d Mon Sep 17 00:00:00 2001 From: Nicolas Hiillos Date: Wed, 19 Oct 2022 09:17:52 +0300 Subject: [PATCH] Implement svg file drawing --- README.md | 12 +++++++++++- src/draw_svg/package.xml | 1 + src/draw_svg/rebuild.sh | 1 + src/draw_svg/src/py/draw_svg.py | 34 ++++++++++++++++++++++++++++++--- src/draw_svg/src/py/readsvg.py | 22 +++++++++++++++++++++ src/draw_svg/svg/test.svg | 5 +++++ 6 files changed, 71 insertions(+), 4 deletions(-) create mode 100644 src/draw_svg/src/py/readsvg.py create mode 100644 src/draw_svg/svg/test.svg 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 @@ + + + + +