def astest():
t = GPS(map_width=100,
map_length=100,
resolution=10,
start_x=50,
start_y=0)
target = (50, 20)
obstacles = scanner.mapping_scan()
print(obstacles)
#obstacles[:,0] *= -1
#print(obstacles)
# populate map with obstacles
for obst in obstacles:
picar_orientation = 0
# actual orientation = picar_orientation + obstacle_scan angle
orientation = obst[0] + picar_orientation
t.add_relative_obstacle(orientation=obst[0], distance=obst[1])
t.save_grid('maps/1object_scan_result.out')
instructions = t.set_navigation_goal(target)
# while not at target
while (len(instructions) > 0):
# convert instructions to polar
step = instructions.pop()
print(step)
def stationary_scan_test():
scan_list = scanner.scan_step_dist()
while not scan_list:
scan_list = scanner.scan_step_dist()
print(scan_list)
grid = np.zeros([50, 50], dtype=int)
t = GPS()
t.load_grid(grid, resolution=2, start_x=0, start_y=0)
# performs a full 180 deg scan at 5 deg intervals
obstacles = scanner.mapping_scan()
print(obstacles)
# populate map with obstacles
for obst in obstacles:
picar_orientation = 0
# actual orientation = picar_orientation + obstacle_scan angle
orientation = obst[0] + picar_orientation
t.add_relative_obstacle(orientation=orientation, distance=obst[1])
# save the scan results to file
t.save_grid('maps/1object_scan_result.out')
return t
