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.lines.append((p1,p2))
self.svg_processor = SVGProcessor(self.get_logger()) 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): def send_goal(self, motion):
self.busy = True self.busy = True
@@ -117,37 +118,33 @@ class DrawingController(Node):
feedback = feedback_msg.feedback feedback = feedback_msg.feedback
self.get_logger().info('Received feedback: {0}'.format(feedback)) self.get_logger().info('Received feedback: {0}'.format(feedback))
def append_point(self, motion, point, height): def append_points(self, motion, points):
p = Pose() for point in points:
#self.get_logger().info('Appending point:{} {}'.format(point[0], point[1])) p = Pose()
p.position.x = point[0] #self.get_logger().info('Appending point:{} {}'.format(point[0], point[1]))
p.position.y = point[1] p.position.x = float(point[0])
p.position.z = height p.position.y = float(point[1])
q = quaternion_from_euler(0.0, math.pi, 0.0) p.position.z = float(point[2])
p.orientation = Quaternion() q = quaternion_from_euler(0.0, math.pi, 0.0)
p.orientation.x = q[0] p.orientation = Quaternion()
p.orientation.y = q[1] p.orientation.x = q[0]
p.orientation.z = q[2] p.orientation.y = q[1]
p.orientation.w = q[3] p.orientation.z = q[2]
ps = PoseStamped() p.orientation.w = q[3]
ps.pose = p ps = PoseStamped()
motion.path.append(ps) ps.pose = p
motion.path.append(ps)
def timer_callback(self): def timer_callback(self):
if self.busy: if self.busy:
return return
next_line = self.lines[self.i] next_motion = self.svg[self.i]
motion = Motion() motion = Motion()
p1 = self.map_point(next_line[0][0],next_line[0][1]) self.append_points(motion, next_motion)
p2 = self.map_point(next_line[1][0],next_line[1][1]) self.i = self.i + 1
self.get_logger().info('Drawing line with p1:{} p2:{}'.format(p1,p2)) if self.i >= len(self.svg):
self.append_point(motion, p1, 1.0) exit()
self.append_point(motion, p1, 0.0) self.get_logger().info('Executing motion: {}...'.format(motion.path[:10]))
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.send_goal(motion) 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) p = self.map_point(x,y)
output.append((p[0],p[1],0.0)) output.append((p[0],p[1],0.0))
while i < len(path): while i < len(path):
w = path[i] w = path[i]
# MoveTo commands # MoveTo commands
@@ -176,20 +177,21 @@ class SVGProcessor():
self.logger.error("SVG path parser '{}' not implemented".format(w)) self.logger.error("SVG path parser '{}' not implemented".format(w))
# Cubic Bézier Curve commands # Cubic Bézier Curve commands
if (w == "C"): if (w == "C"):
self.logger.info("SVG path parser cubic bezier curve at i={}".format(i))
while True: while True:
# https://github.com/sintef/Splipy/tree/master/examples # https://github.com/sintef/Splipy/tree/master/examples
control_points = [(x,y), control_points = [(x,y),
(getnum(),getnum()), (getnum(),getnum()),
(getnum(),getnum()), (getnum(),getnum()),
(getnum(),getnum())] (getnum(),getnum())]
x = control_points[-1][0]
y = control_points[-1][1]
control_points = np.array(control_points) control_points = np.array(control_points)
n = 10 n = 10
curve = cf.cubic_curve(control_points) curve = cf.cubic_curve(control_points)
lin = np.linspace(curve.start(0), curve.end(0), n) 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) appendpoints(coordinates)
if not nextisnum(): if not nextisnum():
break break
@@ -216,14 +218,15 @@ class SVGProcessor():
if (w == "a"): if (w == "a"):
self.logger.error("SVG path parser '{}' not implemented".format(w)) self.logger.error("SVG path parser '{}' not implemented".format(w))
# ClosePath commands # ClosePath commands
if (w == "Z"): if (w == "Z" or w == "z"):
self.logger.error("SVG path parser '{}' not implemented".format(w)) #TODO draw line if start and end point not are the same
if (w == "z"): i += 1
self.logger.error("SVG path parser '{}' not implemented".format(w)) continue
self.logger.error("SVG path parser panic mode at '{}'".format(w)) self.logger.error("SVG path parser panic mode at '{}'".format(w))
i += 1 i += 1
self.logger.info("Finished parsing path") self.logger.info("Finished parsing path :'{}...' with {} points".format(output[:20], len(output)))
return output return output
# https://stackoverflow.com/questions/30232031/how-can-i-strip-namespaces-out-of-an-lxml-tree # 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 primitive_fn = self.primitive_line
# path can consist of multiple primitives # path can consist of multiple primitives
if (child.tag == 'path'): if (child.tag == 'path'):
for m in self.path_parser(child): #for m in self.path_parser(child):
motions.append(m) # motions.append(m)
motions.append(self.path_parser(child))
else: else:
primitive_fn = self.get_primitive_fn(child) primitive_fn = self.get_primitive_fn(child)
motions.append(primitive_fn(child)) motions.append(primitive_fn(child))
@@ -267,7 +271,7 @@ class SVGProcessor():
for m in motions: for m in motions:
if m == []: if m == []:
continue continue
self.logger.info("Refining:'{}'".format(m)) #self.logger.info("Refining:'{}...'".format(m[:3]))
motions_refined.append(self.down_and_up(m)) motions_refined.append(self.down_and_up(m))
return motions_refined return motions_refined