def update_boundaries(frame):
# Filter Data
cones = get_cones(frame, LEFT_POLYS, RIGHT_POLYS)
plot_frame(frame, cones)
# Predict Boundaries
predicted_left, predicted_right = predict(LEFT_BOUNDARY, RIGHT_BOUNDARY,
CURR_SPEED, CURR_BEARING)
if predicted_left and predicted_right:
set_boundaries(predicted_left, predicted_right)
# Detect Finish Line
detect_finish_line(cones)
# Create Boundary Lines
left_boundary, right_boundary = create_boundary_lines(cones)
left_boundary, right_boundary = update(left_boundary, right_boundary,
LEFT_BOUNDARY, RIGHT_BOUNDARY)
set_boundaries(left_boundary, right_boundary)
left_xs, left_ys = separate_xy(LEFT_BOUNDARY)
plot_line(LEFT_COEFS, min(left_ys), max(left_ys))
right_xs, right_ys = separate_xy(RIGHT_BOUNDARY)
plot_line(RIGHT_COEFS, min(right_ys), max(right_ys))
def boundaryMapping(self):
'''
Runs the boundary mapping algorithm. Sets left and right bound lists.
'''
# Error Checking
if len(self.detected_cones) < 2:
print(
'Not enough cones detected! Implement short term memory if you want this to work'
)
self.left_bound = []
self.right_bound = []
return
# Format cones list for algorithm
cones_list = []
for point in self.detected_cones:
cones_list.append(point[0])
count_lap = fl.detect_finish_line(cones_list)
self.lap_num = self.lap_num + int(count_lap)
# Run boundary mapping algorithms
bm_on = True
try:
while bm_on:
self.right_bound, self.left_bound = bm.create_boundary_lines(
list(cones_list))
if (len(self.left_bound) + len(self.right_bound) +
2) < len(cones_list):
if len(self.left_bound) < len(self.right_bound):
cones_list.pop(0)
else:
cones_list.pop()
else:
bm_on = False
except Exception, e:
print('Error running boundary mapping!')
traceback.print_exc()
def get_speed_steering(cones):
global LAP_COUNT
if detect_finish_line(cones):
#print '########## FINISH LINE ##########'
LAP_COUNT += 1
left_boundary, right_boundary = create_boundary_lines(cones)
#left_boundary, right_boundary = update(left_boundary, right_boundary,
# LEFT_BOUNDARY, RIGHT_BOUNDARY)
set_boundaries(left_boundary, right_boundary)
#print 'Updated left: ', sorted(LEFT_BOUNDARY)
#print 'Updated right: ', sorted(RIGHT_BOUNDARY)
#print
#speed = get_next_speed(LEFT_BOUNDARY, RIGHT_BOUNDARY, LAP_COUNT)
speed = get_next_speed(left_boundary, right_boundary, LAP_COUNT)
#bearing = boundaries_to_steering(LEFT_BOUNDARY, RIGHT_BOUNDARY)
bearing = boundaries_to_steering(left_boundary, right_boundary)
return speed, bearing
return left_bound, right_bound
if __name__ == '__main__':
if len(sys.argv) > 1:
files = sys.argv[1:]
else:
files = [
'lidar_data/' + path for path in sorted(os.listdir('lidar_data'))
]
for filename in files:
data = parse_csv_data(filename, fov=200)
start = time.time()
for frame in data:
cones = get_cones(frame, [], [])
plot_cones = list(cones)
detect_finish_line(cones)
lines = create_boundary_lines(cones)
print('Boundary mapping took %f seconds' % (time.time() - start))
# Uncomment to see boundaries on a scatter plot
plot_boundaries(plot_cones, lines[0], lines[1]).show()
print 'Left Boundary: ', lines[0]
print 'Right Boundary: ', lines[1]