diff --git a/src/drawing_controller/drawing_controller/drawing_controller.py b/src/drawing_controller/drawing_controller/drawing_controller.py index 678a1a5..173f366 100644 --- a/src/drawing_controller/drawing_controller/drawing_controller.py +++ b/src/drawing_controller/drawing_controller/drawing_controller.py @@ -17,6 +17,8 @@ from robot_interfaces.msg import Motion import sys from copy import deepcopy +import time + from drawing_controller.svg_processor import SVGProcessor def quaternion_from_euler(ai, aj, ak): @@ -61,13 +63,17 @@ class DrawingController(Node): def __init__(self, svgpath): super().__init__('drawing_controller') #self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10) - timer_period = 1.0 # seconds + timer_period = 0.1 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) self.i = 0 self.busy = False self._action_client = ActionClient(self, ExecuteMotion, 'execute_motion') + self.results = {} + + self.svg_start_time = time.time() + xml = ET.parse(svgpath) svg = xml.getroot() #self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2) @@ -85,7 +91,10 @@ class DrawingController(Node): self.svg_processor = SVGProcessor(self.get_logger()) self.svg = self.svg_processor.process_svg(svgpath) - self.get_logger().info('Ready to begin executing motions') + + self.results['svg processing time'] = self.timestr(time.time() - self.svg_start_time) + self.start_time = time.time() + self.get_logger().info('SVG processing finished, executing motions') def send_goal(self, motion): self.busy = True @@ -101,22 +110,23 @@ class DrawingController(Node): def goal_response_callback(self, future): goal_handle = future.result() if not goal_handle.accepted: - self.get_logger().info('Goal rejected :(') + self.get_logger().debug('Goal rejected :(') return - self.get_logger().info('Goal accepted :)') + self.get_logger().debug('Goal accepted :)') self._get_result_future = goal_handle.get_result_async() self._get_result_future.add_done_callback(self.get_result_callback) def get_result_callback(self, future): result = future.result().result - self.get_logger().info('Result: {0}'.format(result)) + self.results.update({ result.result: self.results.get(result.result, 0) + 1 }) + self.get_logger().info('Result: {0}'.format(result.result)) self.busy = False def feedback_callback(self, feedback_msg): feedback = feedback_msg.feedback - self.get_logger().info('Received feedback: {0}'.format(feedback)) + self.get_logger().debug('Received feedback: {0}'.format(feedback)) def append_points(self, motion, points): for point in points: @@ -135,17 +145,23 @@ class DrawingController(Node): ps.pose = p motion.path.append(ps) + def timestr(self, t): + return time.strftime("%H:%M:%S", time.gmtime(t)) + def timer_callback(self): if self.busy: return if self.i >= len(self.svg): + self.results['total time'] = self.timestr(time.time() - self.svg_start_time) + self.results['drawing time'] = self.timestr(time.time() - self.start_time) self.get_logger().info('Finished executing all motions from SVG') + self.get_logger().info('Results: {}'.format(self.results)) exit() next_motion = self.svg[self.i] motion = Motion() self.append_points(motion, next_motion) self.i = self.i + 1 - self.get_logger().info('Executing motion: {}...'.format(motion.path[:10])) + self.get_logger().info('Executing motion: {}/{}'.format(self.i, len(self.svg))) self.send_goal(motion) diff --git a/src/drawing_controller/drawing_controller/svg_path_parser.py b/src/drawing_controller/drawing_controller/svg_path_parser.py index 2bac2e2..5c23bf3 100644 --- a/src/drawing_controller/drawing_controller/svg_path_parser.py +++ b/src/drawing_controller/drawing_controller/svg_path_parser.py @@ -11,7 +11,7 @@ class SVGPathParser(): self.map_point = map_point def tokenize(self, pathstr): - self.logger.info("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr))) + self.logger.debug("Tokenizing path :'{}...' with {} characters".format(pathstr[:40], len(pathstr))) path = [] i = 0 while i < len(pathstr): @@ -53,7 +53,7 @@ class SVGPathParser(): Returns: primitive_fn (): ''' - self.logger.info("Parsing path :'{}...' with {} tokens".format(path[:20], len(path))) + self.logger.debug("Parsing path :'{}...' with {} tokens".format(path[:20], len(path))) x = 0.0 y = 0.0 i = 0 @@ -289,5 +289,5 @@ class SVGPathParser(): self.logger.error("SVG path parser panic mode at '{}'".format(w)) i += 1 - self.logger.info("Finished parsing path :'{}...' with {} points".format(output[:3], len(output))) + self.logger.debug("Finished parsing path :'{}...' with {} points".format(output[:3], len(output))) return output diff --git a/src/robot_controller/src/cpp/dummy_controller.cpp b/src/robot_controller/src/cpp/dummy_controller.cpp index cbc5cfb..0d311ef 100644 --- a/src/robot_controller/src/cpp/dummy_controller.cpp +++ b/src/robot_controller/src/cpp/dummy_controller.cpp @@ -23,7 +23,7 @@ class DummyController : public RobotController virtual void executePath(const std::shared_ptr> goal_handle) { RCLCPP_INFO(this->get_logger(), "Executing goal"); - rclcpp::Rate loop_rate(20); + rclcpp::Rate loop_rate(100); const auto goal = goal_handle->get_goal(); auto feedback = std::make_shared(); auto result = std::make_shared();