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 parse_and_launch(): """parse sys.argv arguments and return initialized robot""" parser = argparse.ArgumentParser( description='Navigate given pattern in selected area') subparsers = parser.add_subparsers(help='sub-command help', dest='command') subparsers.required = True parser_run = subparsers.add_parser('run', help='run on real HW') parser_run.add_argument('config', help='configuration file') parser_run.add_argument('--note', help='add description') parser_replay = subparsers.add_parser('replay', help='replay from logfile') parser_replay.add_argument('logfile', help='recorded log file') parser_replay.add_argument('--view', dest='view', action='store_true', help='view parsed log') parser_replay.add_argument( '--force', '-F', dest='force', action='store_true', help='force replay even for failing output asserts') parser_replay.add_argument('--config', dest='config', help='use different configuration file') args = parser.parse_args() conf = None if args.config is not None: conf = Config.load(args.config) viewer = None if args.command == 'replay': metalog = MetaLog(args.logfile) if conf is None and args.logfile.endswith('.zip'): if DEFAULT_ZIP_CONFIG in zipfile.ZipFile(args.logfile).namelist(): s = str( zipfile.ZipFile(args.logfile).read(DEFAULT_ZIP_CONFIG), 'utf-8') conf = Config.loads(s) if args.view: global g_img_dir from tools.viewer import main as viewer_main from tools.viewer import getCombinedPose viewer = viewer_main if args.logfile.endswith('.zip'): g_img_dir = args.logfile else: g_img_dir = os.path.dirname(args.logfile) if args.force: disableAsserts() elif args.command == 'run': metalog = MetaLog() else: assert False, args.command # unsupported command robot = create_robot(metalog, conf) for sensor_name in ['gps', 'laser', 'camera']: attach_sensor(robot, sensor_name, metalog) if viewer is not None: robot.extensions.append(('launcher_viewer', launcher_viewer_extension)) try: robot.extensions.append(('emergency_stop', emergency_stop_extension)) yield robot, metalog, conf, viewer except EmergencyStopException: print("Emergency STOP Exception!") robot.extensions = [] # hack # TODO make this code conditional based on the state of finished application robot.canproxy.stop() robot.canproxy.stop_turn() robot.wait(3.0) detach_all_sensors(robot) if viewer is not None: viewer(filename=None, posesScanSet=viewer_data)
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)
def parse_and_launch(): """parse sys.argv arguments and return initialized robot""" parser = argparse.ArgumentParser(description='Navigate given pattern in selected area') subparsers = parser.add_subparsers(help='sub-command help', dest='command') subparsers.required = True parser_run = subparsers.add_parser('run', help='run on real HW') parser_run.add_argument('config', help='configuration file') parser_run.add_argument('--note', help='add description') parser_replay = subparsers.add_parser('replay', help='replay from logfile') parser_replay.add_argument('logfile', help='recorded log file') parser_replay.add_argument('--view', dest='view', action='store_true', help='view parsed log') parser_replay.add_argument('--force', '-F', dest='force', action='store_true', help='force replay even for failing output asserts') parser_replay.add_argument('--config', dest='config', help='use different configuration file') args = parser.parse_args() conf = None if args.config is not None: conf = Config.load(args.config) viewer = None if args.command == 'replay': metalog = MetaLog(args.logfile) if conf is None and args.logfile.endswith('.zip'): if DEFAULT_ZIP_CONFIG in zipfile.ZipFile(args.logfile).namelist(): s = str(zipfile.ZipFile(args.logfile).read(DEFAULT_ZIP_CONFIG), 'utf-8') conf = Config.loads(s) if args.view: global g_img_dir from tools.viewer import main as viewer_main from tools.viewer import getCombinedPose viewer = viewer_main if args.logfile.endswith('.zip'): g_img_dir = args.logfile else: g_img_dir = os.path.dirname(args.logfile) if args.force: disableAsserts() elif args.command == 'run': metalog = MetaLog() else: assert False, args.command # unsupported command robot = create_robot(metalog, conf) for sensor_name in ['gps', 'laser', 'camera']: attach_sensor(robot, sensor_name, metalog) if viewer is not None: robot.extensions.append(('launcher_viewer', launcher_viewer_extension)) try: robot.extensions.append(('emergency_stop', emergency_stop_extension)) yield robot, metalog, conf, viewer except EmergencyStopException: print("Emergency STOP Exception!") robot.extensions = [] # hack # TODO make this code conditional based on the state of finished application robot.canproxy.stop() robot.canproxy.stop_turn() robot.wait(3.0) detach_all_sensors(robot) if viewer is not None: viewer(filename=None, posesScanSet=viewer_data)
def demo(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) robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup robot.localization = None # TODO for sensor_name in ['gps', 'laser', 'camera']: attach_sensor(robot, sensor_name, metalog) print "Wait for gas" while robot.canproxy.gas is None: robot.update() print "Wait for center" robot.canproxy.stop() # not available now :( ... wait_for_start(robot) moving = False robot.desired_speed = 0.5 start_time = robot.time prev_gps = robot.gps_data prev_destination_dist = None last_laser_update = None prev_laser = None while robot.time - start_time < 3 * 60: # 3min robot.update() dist = None turn_angle = None 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, distR = min_dist_arr(robot.laser_data[200:-200]) distL = 20.0 if distL is None else distL distR = 20.0 if distR is None else distR dist = min(distL, distR) turn_angle = follow_wall_angle(robot.laser_data, radius=1.5) 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 if robot.gps_data != prev_gps: if turn_angle is not None: print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % ( distL, distR), math.degrees(turn_angle) else: print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % ( distL, distR), turn_angle prev_gps = robot.gps_data if moving: if dist is None or dist < SAFE_DISTANCE_STOP: print "!!! STOP !!!", dist, (distL, distR) robot.canproxy.stop() moving = False else: # not moving if dist is not None and dist > SAFE_DISTANCE_GO: print "GO", dist robot.set_desired_speed(DESIRED_SPEED) moving = True if turn_angle is not None: turn_cmd = max(-120, min(120, 2 * math.degrees(turn_angle))) robot.canproxy.set_turn_raw(turn_cmd) # print turn_cmd # if not robot.buttonGo: # print "STOP!" # break robot.canproxy.stop_turn() robot.canproxy.stop() for i in xrange(20): robot.update() detach_all_sensors(robot)
def demo(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) robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup robot.localization = None # TODO for sensor_name in ['gps', 'laser', 'camera']: attach_sensor(robot, sensor_name, metalog) print("Wait for gas") while robot.canproxy.gas is None: robot.update() print("Wait for center") robot.canproxy.stop() # not available now :( ... wait_for_start(robot) moving = False robot.desired_speed = 0.5 start_time = robot.time prev_gps = robot.gps_data prev_destination_dist = None last_laser_update = None prev_laser = None while robot.time - start_time < 3*60: # 3min robot.update() dist = None turn_angle = None 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, distR = min_dist_arr(robot.laser_data[200:-200]) distL = 20.0 if distL is None else distL distR = 20.0 if distR is None else distR dist = min(distL, distR) turn_angle = follow_wall_angle(robot.laser_data, radius=1.5) 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 if robot.gps_data != prev_gps: if turn_angle is not None: print(robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), math.degrees(turn_angle)) else: print(robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), turn_angle) prev_gps = robot.gps_data if moving: if dist is None or dist < SAFE_DISTANCE_STOP: print("!!! STOP !!!", dist, (distL, distR)) robot.canproxy.stop() moving = False else: # not moving if dist is not None and dist > SAFE_DISTANCE_GO: print("GO", dist) robot.set_desired_speed(DESIRED_SPEED) moving = True if turn_angle is not None: turn_cmd = max(-120, min(120, 2*math.degrees(turn_angle))) robot.canproxy.set_turn_raw(turn_cmd) # print turn_cmd # if not robot.buttonGo: # print "STOP!" # break robot.canproxy.stop_turn() robot.canproxy.stop() for i in range(20): robot.update() detach_all_sensors(robot)