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 = []
if len(sys.argv) > 2: start_index = int(sys.argv[2]) else: first_index = 1000 if len(sys.argv) > 3: seconds_per_plot = int(sys.argv[3]) lidar_viewer = LidarViewer() file_index = start_index while 1: fname = "{:s}/lidar_snapshot_{:d}.dat".format(image_directory, file_index) lidar_data = [] print("Loading file: {:s}\n".format(fname)) lidar_data = np.loadtxt(fname, delimiter=",") # toss out the error data error_free_data = [] if len(lidar_data) > 0: error_free_data = np.array(filter(lambda x: x[1] < 777, lidar_data)) if len(error_free_data) > 0: print(error_free_data) pdb.set_trace() lidar_viewer.plot_polar(error_free_data) file_index = file_index + 1
text = raw_input("Enter command (mOVE,tURN,sHOW,iNFO,0(origin),hELP or qQUIT): ") cmd = text.split(" ")[0] #If we see a quit command, well, quit if cmd == "q": break #Reset robot to the origin with 0 turn if cmd == "0": robot = Robot((0,0), 0) elif cmd == "i": x,y = robot.position print("Robot is at ({:f}, {:f} turned {:d} degrees".format(x, y, r.heading)) elif cmd == "s": rotation = FakeRotation(field_model, robot) lidar_viewer.plot_polar(rotation.polar_data()) elif cmd == "m": try: cmd,cmd_arg = text.split(" ") distance = int(cmd_arg) robot.move(distance) rotation = FakeRotation(field_model, robot) sweep_heading, sweep_range = Analyzer.range_at_heading(rotation.polar_data(), (-5, 6)) print("Closest point in (-5,6) sweep is ({:d},{:.2f})".format(sweep_heading, sweep_range)) lidar_viewer.plot_polar(rotation.polar_data()) except ValueError: print "Please specify distance in whole inches." elif cmd == "t": try: cmd,cmd_arg = text.split(" ") heading = int(cmd_arg)