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)
