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)
