def navigate_pattern(metalog, conf, viewer=None): assert metalog is not None can_log_name = metalog.getLog('can') if metalog.replay: if metalog.areAssertsEnabled(): can = CAN(ReplayLog(can_log_name), skipInit=True) else: can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True) else: can = CAN() can.relog(can_log_name, timestamps_log=open(metalog.getLog('timestamps'), 'w')) can.resetModules(configFn=setup_faster_update) if conf is not None and 'localization' in conf.data: loc = SimpleOdometry.from_dict(conf.data['localization']) else: loc = SimpleOdometry(pose=(0.0, 2.5, 0.0)) loc.global_map = [(0.0, 0.0), (15.0, 0.0), (15.0, 5.0), (0.0, 5.0)] jd_config = None if conf is not None and 'johndeere' in conf.data: jd_config = conf.data['johndeere'] robot = JohnDeere(can=can, localization=loc, config=jd_config) robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup for sensor_name in ['gps', 'laser', 'camera']: attach_sensor(robot, sensor_name, metalog) attach_processor(robot, metalog, image_callback) long_side = max([x for x, y in robot.localization.global_map]) robot.canproxy.stop() robot.canproxy.set_turn_raw(0) if viewer is not None: robot.extensions.append(('viewer', viewer_extension)) speed = 0.5 try: robot.extensions.append(('detect_near', detect_near_extension)) robot.extensions.append(('emergency_stop', emergency_stop_extension)) for i in xrange(10): # run_oval(robot, speed) run_there_and_back(robot, long_side, speed) except NearObstacle: print "Near Exception Raised!" robot.extensions = [] # hack except EmergencyStopException: print "Emergency STOP Exception!" robot.extensions = [] # hack robot.canproxy.stop() robot.canproxy.stop_turn() robot.wait(3.0) detach_all_sensors(robot)
def robot_go_straight(metalog): assert metalog is not None can_log_name = metalog.getLog('can') if metalog.replay: if metalog.areAssertsEnabled(): can = CAN(ReplayLog(can_log_name), skipInit=True) else: can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True) else: can = CAN() can.relog(can_log_name, timestamps_log=open(metalog.getLog('timestamps'), 'w')) can.resetModules(configFn=setup_faster_update) robot = JohnDeere(can=can, localization=SimpleOdometry()) robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup for sensor_name in ['gps', 'laser', 'camera']: attach_sensor(robot, sensor_name, metalog) robot.canproxy.stop() robot.set_desired_steering(0.0) # i.e. go straight (!) try: robot.extensions.append(('emergency_stop', emergency_stop_extension)) print(robot.canproxy.buttons_and_LEDs) wait_for_start(robot) print(robot.canproxy.buttons_and_LEDs) prev_laser = None last_laser_update = None moving = False dist = None distL, distR = None, None prev_camera = None global_offset = 0.0 while True: robot.update() if robot.laser_data is not None: assert len(robot.laser_data) == 541, len(robot.laser_data) if robot.laser_data != prev_laser: prev_laser = robot.laser_data last_laser_update = robot.time distL = min_dist(robot.laser_data[:180], INFINITY) dist = min_dist(robot.laser_data[180:360], INFINITY) distR = min_dist(robot.laser_data[360:], INFINITY) print("dist", distL, dist, distR) if prev_camera != robot.camera_data: print("CAMERA", robot.camera_data) prev_camera = robot.camera_data # filename = 'm:\\git\\osgar\\logs\\pisek170513\\game3\\' + os.path.basename(prev_camera[0]) filename = prev_camera[0] img = cv2.imread(filename) if img is not None: img = img[2*768/3:,:,:] r = img[:,:,0] g = img[:,:,1] b = img[:,:,2] mask = np.logical_and(g > b, g > r) img[mask] = 0 left = mask[:, :512].sum() right = mask[:, 512:].sum() # print "LEFT_RIGHT", filename, left, right if left > GREEN_LIMIT or right > GREEN_LIMIT: if left < right: global_offset = math.radians(2.5) else: global_offset = math.radians(-2.5) else: global_offset = 0.0 robot.set_desired_steering(0.0 + global_offset) if moving: if dist is None or dist < SAFE_DISTANCE_STOP or min(distL, distR) < SAFE_SIDE_DISTANCE_STOP: print("!!! STOP !!!", distL, dist, distR) robot.canproxy.stop() moving = False else: # not moving if dist is not None and dist > SAFE_DISTANCE_GO and min(distL, distR) > SAFE_SIDE_DISTANCE_GO: print("GO", distL, dist, distR) robot.set_desired_speed(DESIRED_SPEED) moving = True if last_laser_update is not None and robot.time - last_laser_update > 0.3: print("!!!WARNING!!! Missing laser updates for last {:.1f}s".format(robot.time - last_laser_update)) dist = None # no longer valid distance measurements except EmergencyStopException: print("Emergency STOP Exception!") robot.extensions = [] # hack robot.canproxy.stop() robot.canproxy.stop_turn() robot.wait(3.0) detach_all_sensors(robot)