diff --git a/src/drawing_controller/drawing_controller/drawing_controller.py b/src/drawing_controller/drawing_controller/drawing_controller.py index f39b682..f28806d 100644 --- a/src/drawing_controller/drawing_controller/drawing_controller.py +++ b/src/drawing_controller/drawing_controller/drawing_controller.py @@ -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) diff --git a/src/drawing_controller/drawing_controller/svg_processor.py b/src/drawing_controller/drawing_controller/svg_processor.py index 4765c4a..0231c2d 100644 --- a/src/drawing_controller/drawing_controller/svg_processor.py +++ b/src/drawing_controller/drawing_controller/svg_processor.py @@ -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