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
def drive_target(nav: GPS, target: tuple): car_theta = 0 curr_distance = 0 picar = Car() at_destination = False while (not at_destination): # scan for obstacles obstacles = scanner.mapping_scan() print(obstacles) for obst in obstacles: abs_orient = obst[0] + picar.orientation nav.add_relative_obstacle(orientation=abs_orient, distance=obst[1]) instructions = nav.set_navigation_goal(target) at_destination = drive_instructions(picar, nav, instructions)
class SimplePiCar(Car): def __init__(self): super().__init__() self.nav = GPS(map_width = 100, map_length = 100, resolution = 10, start_x = 50, start_y = 0) self.obstacle_queue = Queue() self.cam = detect.TrafficCam() def drive_step(self, distance: int, power: int = 2): self.trip_meter.reset() while(self.cam.detect_traffic() == True): fc.stop() print("Obstacle detected") time.sleep(2) while(self.trip_meter.distance < distance): fc.forward(2) fc.stop() print(self.trip_meter.distance, distance) # drives towards a target def drive_target(self, target: tuple): at_destination = False while(target != self.nav.position): self.nav.clear_grid() # scan for obstacles obstacles = scanner.mapping_scan() print(self.orientation) print(obstacles) for obst in obstacles: abs_orient = obst[0] + self.orientation self.nav.add_relative_obstacle(orientation = abs_orient, distance = obst[1]) instructions = self.nav.set_navigation_goal(target) if len(instructions) == 0: print("unable to find path") return # take the first 3 instructions print(target, self.nav.position) self.execute_instructions(instructions) print("arrived at destination") # drive according to instructions until blocked or finished def execute_instructions(self, instructions:deque): # while not at target steps_taken = 0 while(len(instructions) > 0 and steps_taken< 3): # convert instructions to polar step = instructions.popleft() print("directions: ", step) # calculate the shortest angle to turn direction = step[0] - self.orientation direction = (direction + 180) % 360 - 180 #print("turning angle", direction) # change direction if needed if direction > 0: self.turn_right(direction) elif direction < 0: self.turn_left(abs(direction)) steps_taken += 1 self.drive_step(distance = step[1]) # update position self.nav.update_postion(step[1], self.orientation)