nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Switch drawing controller to use new svg_processor

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-02-08 11:02:26 +02:00
parent a88f3f9060
commit f18f1de042
2 changed files with 40 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
src/drawing_controller/drawing_controller/drawing_controller.py
View File

@@ -84,7 +84,8 @@ class DrawingController(Node):
self.lines.append((p1,p2))
self.svg_processor = SVGProcessor(self.get_logger())
print(self.svg_processor.process_svg(svgpath))
self.svg = self.svg_processor.process_svg(svgpath)
self.get_logger().info('Ready to begin executing motions')
def send_goal(self, motion):
self.busy = True
@@ -117,37 +118,33 @@ class DrawingController(Node):
feedback = feedback_msg.feedback
self.get_logger().info('Received feedback: {0}'.format(feedback))
def append_point(self, motion, point, height):
p = Pose()
#self.get_logger().info('Appending point:{} {}'.format(point[0], point[1]))
p.position.x = point[0]
p.position.y = point[1]
p.position.z = height
q = quaternion_from_euler(0.0, math.pi, 0.0)
p.orientation = Quaternion()
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
ps = PoseStamped()
ps.pose = p
motion.path.append(ps)
def append_points(self, motion, points):
for point in points:
p = Pose()
#self.get_logger().info('Appending point:{} {}'.format(point[0], point[1]))
p.position.x = float(point[0])
p.position.y = float(point[1])
p.position.z = float(point[2])
q = quaternion_from_euler(0.0, math.pi, 0.0)
p.orientation = Quaternion()
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
ps = PoseStamped()
ps.pose = p
motion.path.append(ps)
def timer_callback(self):
if self.busy:
return
next_line = self.lines[self.i]
next_motion = self.svg[self.i]
motion = Motion()
p1 = self.map_point(next_line[0][0],next_line[0][1])
p2 = self.map_point(next_line[1][0],next_line[1][1])
self.get_logger().info('Drawing line with p1:{} p2:{}'.format(p1,p2))
self.append_point(motion, p1, 1.0)
self.append_point(motion, p1, 0.0)
self.append_point(motion, p2, 0.0)
self.append_point(motion, p2, 1.0)
self.i = (self.i + 1) % len(self.lines)
self.get_logger().info('Executing motion:{}'.format(motion.path))
self.append_points(motion, next_motion)
self.i = self.i + 1
if self.i >= len(self.svg):
exit()
self.get_logger().info('Executing motion: {}...'.format(motion.path[:10]))
self.send_goal(motion)

28
src/drawing_controller/drawing_controller/svg_processor.py
View File

@@ -144,6 +144,7 @@ class SVGProcessor():
p = self.map_point(x,y)
output.append((p[0],p[1],0.0))
while i < len(path):
w = path[i]
# MoveTo commands
@@ -176,20 +177,21 @@ class SVGProcessor():
self.logger.error("SVG path parser '{}' not implemented".format(w))
# Cubic Bézier Curve commands
if (w == "C"):
self.logger.info("SVG path parser cubic bezier curve at i={}".format(i))
while True:
# https://github.com/sintef/Splipy/tree/master/examples
control_points = [(x,y),
(getnum(),getnum()),
(getnum(),getnum()),
(getnum(),getnum())]
x = control_points[-1][0]
y = control_points[-1][1]
control_points = np.array(control_points)
n = 10
curve = cf.cubic_curve(control_points)
lin = np.linspace(curve.start(0), curve.end(0), n)
coordinates = curve(lin) # physical (x,y)-coordinates, size (n,2)
coordinates = curve(lin)
coordinates = np.nan_to_num(coordinates)
#self.logger.info("Appending curve points: {}".format(coordinates))
x = coordinates[-1][0]
y = coordinates[-1][1]
appendpoints(coordinates)
if not nextisnum():
break
@@ -216,14 +218,15 @@ class SVGProcessor():
if (w == "a"):
self.logger.error("SVG path parser '{}' not implemented".format(w))
# ClosePath commands
if (w == "Z"):
self.logger.error("SVG path parser '{}' not implemented".format(w))
if (w == "z"):
self.logger.error("SVG path parser '{}' not implemented".format(w))
if (w == "Z" or w == "z"):
#TODO draw line if start and end point not are the same
i += 1
continue
self.logger.error("SVG path parser panic mode at '{}'".format(w))
i += 1
self.logger.info("Finished parsing path")
self.logger.info("Finished parsing path :'{}...' with {} points".format(output[:20], len(output)))
return output
# https://stackoverflow.com/questions/30232031/how-can-i-strip-namespaces-out-of-an-lxml-tree
@@ -257,8 +260,9 @@ class SVGProcessor():
primitive_fn = self.primitive_line
# path can consist of multiple primitives
if (child.tag == 'path'):
for m in self.path_parser(child):
motions.append(m)
#for m in self.path_parser(child):
# motions.append(m)
motions.append(self.path_parser(child))
else:
primitive_fn = self.get_primitive_fn(child)
motions.append(primitive_fn(child))
@@ -267,7 +271,7 @@ class SVGProcessor():
for m in motions:
if m == []:
continue
self.logger.info("Refining:'{}'".format(m))
#self.logger.info("Refining:'{}...'".format(m[:3]))
motions_refined.append(self.down_and_up(m))
return motions_refined