def drive_remotely(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) 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 soc = metalog.createLoggedSocket('remote', headerFormat=None) # TODO fix headerFormat soc.bind( ('',PORT) ) if not metalog.replay: soc.soc.setblocking(0) soc.soc.settimeout( SOCKET_TIMEOUT ) remote_cmd_log_name = metalog.getLog('remote_cmd') if metalog.replay: robot.remote = DummySensor() function = SourceLogger(None, remote_cmd_log_name).get else: robot.remote = RemoteThread(soc) function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get max_speed = None robot.remote_data = None robot.register_data_source('remote', function, remote_data_extension) robot.remote.start() robot.canproxy.stop() robot.canproxy.set_turn_raw(0) print("Waiting for remote commands ...") while robot.remote_data is None: robot.update() print("received", robot.remote_data.strip()) moving = False turning = False while robot.remote_data != 'END\n': robot.update() if robot.remote_data == 'STOP\n' and (moving or turning): if moving: robot.canproxy.stop() print("STOP") moving = False if turning: robot.canproxy.stop_turn() print("STOP turn") turning = False elif robot.remote_data == 'UP\n' and not moving: if max_speed is None: robot.canproxy.go() print("GO") else: robot.set_desired_speed(max_speed) print("GO", max_speed) moving = True elif robot.remote_data == 'DOWN\n' and not moving: if max_speed is None: robot.canproxy.go_back() print("GO back") else: robot.set_desired_speed(-max_speed) print("GO back", max_speed) moving = True elif robot.remote_data == 'LEFT\n': robot.canproxy.set_turn_raw(200) print("Left") turning = True elif robot.remote_data == 'RIGHT\n': robot.canproxy.set_turn_raw(-200) print("Right") turning = True elif robot.remote_data == 'SPEED0\n': max_speed = None elif robot.remote_data.startswith('SPEED'): max_speed = int(robot.remote_data[len('SPEED'):])/10.0 print("Max Speed", max_speed) print("received", robot.remote_data.strip()) robot.canproxy.stop_turn() robot.remote.requestStop() robot.wait(3.0)
def drive_remotely(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) 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 soc = metalog.createLoggedSocket('remote', headerFormat=None) # TODO fix headerFormat soc.bind( ('',PORT) ) if not metalog.replay: soc.soc.setblocking(0) soc.soc.settimeout( SOCKET_TIMEOUT ) remote_cmd_log_name = metalog.getLog('remote_cmd') if metalog.replay: robot.remote = DummySensor() function = SourceLogger(None, remote_cmd_log_name).get else: robot.remote = RemoteThread(soc) function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get max_speed = None robot.remote_data = None robot.register_data_source('remote', function, remote_data_extension) robot.remote.start() robot.canproxy.stop() robot.canproxy.set_turn_raw(0) print "Waiting for remote commands ..." while robot.remote_data is None: robot.update() print "received", robot.remote_data.strip() moving = False turning = False while robot.remote_data != 'END\n': robot.update() if robot.remote_data == 'STOP\n' and (moving or turning): if moving: robot.canproxy.stop() print "STOP" moving = False if turning: robot.canproxy.stop_turn() print "STOP turn" turning = False elif robot.remote_data == 'UP\n' and not moving: if max_speed is None: robot.canproxy.go() print "GO" else: robot.set_desired_speed(max_speed) print "GO", max_speed moving = True elif robot.remote_data == 'DOWN\n' and not moving: if max_speed is None: robot.canproxy.go_back() print "GO back" else: robot.set_desired_speed(-max_speed) print "GO back", max_speed moving = True elif robot.remote_data == 'LEFT\n': robot.canproxy.set_turn_raw(200) print "Left" turning = True elif robot.remote_data == 'RIGHT\n': robot.canproxy.set_turn_raw(-200) print "Right" turning = True elif robot.remote_data == 'SPEED0\n': max_speed = None elif robot.remote_data.startswith('SPEED'): max_speed = int(robot.remote_data[len('SPEED'):])/10.0 print "Max Speed", max_speed print "received", robot.remote_data.strip() robot.canproxy.stop_turn() robot.remote.requestStop() robot.wait(3.0)
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 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 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)