channel.send_to(periodic_message.encode_message()) logger.error("Failed to gather a full rotation of data.") # # get revised instructions from robot # try: robot_data, robot_address = channel.receive_from() message_from_robot = RobotMessage(robot_data) if (message_from_robot.sender == "robot") and (message_from_robot.message == "sweep"): Analyzer.start = message_from_robot.start Analyzer.stop = message_from_robot.stop except socket.timeout: logger.info("No message received from robot") elapsed_time = SECONDS_PER_MINUTE / float(rotation.rpm()) rotation_time = rotation_time + elapsed_time current_time = current_time + elapsed_time if rotation_time > seconds_per_output: sorted_rot = sorted(list(itertools.chain(*rotations)), key=lambda (theta, _): theta) grouped_by = itertools.groupby(sorted_rot, lambda x: x[0]) groups = [(key, list(items)) for key, items in grouped_by] polar_data = [(angle, aggregate_distance(ranges)) for angle, ranges in groups] lidar_viewer.plot_polar(polar_data) lidar_viewer.plot_cartesian(rotation.cartesian_data()) lidar_logger.log_data(rotation.polar_data()) pdb.set_trace() file_index = file_index + 1 rotation_time = 0 rotations = []
range_at_heading_message.range = tgt_range if not suppress_robot_comm: channel.send_to(range_at_heading_message.encode_message()) # push periodic message to the bot periodic_message.status = 'ok' periodic_message.rpm = rotation.rpm() if not suppress_robot_comm: channel.send_to(periodic_message.encode_message()) logger.info("reported rpm is {:d}".format(rotation.rpm())) # # send out the wall heading and distance report (disabled) # if 0: (wall_heading, wall_distance, wall_orientation) = find_wall_midpoint(rotation.cartesian_data()) logger.info("find_wall_midpoint => heading {:.1f}, range {:.1f}, orientation {:.1f})".format(wall_heading, wall_distance, wall_orientation)) if (wall_heading, wall_distance, wall_orientation) == (0, 0, 0): wall_message.status = 'error' else: wall_message.status = 'ok' wall_message.range = wall_distance wall_message.heading = wall_heading wall_message.orientation = wall_orientation if not suppress_robot_comm: channel.send_to(wall_message.encode_message()) except IOError,e: # log and notify robot of error periodic_message.status = 'error'