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)
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
# GPS
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Laser
laser_log_name = metalog.getLog('laser')
print laser_log_name
if metalog.replay:
robot.laser = DummySensor()
function = SourceLogger(None, laser_log_name).get
else:
robot.laser = LaserIP()
function = SourceLogger(robot.laser.scan, laser_log_name).get
robot.laser_data = None
robot.register_data_source('laser', function, laser_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.laser.start()
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.canproxy.go()
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()
robot.laser.requestStop()
robot.gps.requestStop()
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)
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
# GPS
gps_log_name = metalog.getLog('gps')
print gps_log_name
if metalog.replay:
robot.gps = DummySensor()
function = SourceLogger(None, gps_log_name).get
else:
robot.gps = GPS(verbose=0)
function = SourceLogger(robot.gps.coord, gps_log_name).get
robot.gps_data = None
robot.register_data_source('gps', function, gps_data_extension)
# Laser
laser_log_name = metalog.getLog('laser')
print laser_log_name
if metalog.replay:
robot.laser = DummySensor()
function = SourceLogger(None, laser_log_name).get
else:
robot.laser = LaserIP()
function = SourceLogger(robot.laser.scan, laser_log_name).get
robot.laser_data = None
robot.register_data_source('laser', function, laser_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.laser.start()
while robot.gas is None:
robot.update()
center(robot)
wait_for_start(robot)
moving = False
robot.desired_speed = 0.5
start_time = robot.time
prev_gps = robot.gps_data
prev_destination_dist = None
while robot.time - start_time < 30*60: # limit 30 minutes
robot.update()
dist = None
if robot.laser_data is not None:
assert len(robot.laser_data) == 541, len(robot.laser_data)
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)
if robot.gps_data != prev_gps:
print robot.time, robot.gas, robot.gps_data, (distL, distR)
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
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
print "DECIDE", distL, distR
if distL <= distR:
robot.canproxy.set_turn_raw(-100)
robot.steering_angle = math.radians(-30)
else:
robot.canproxy.set_turn_raw(100)
robot.steering_angle = math.radians(30)
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
robot.canproxy.set_turn_raw(0)
robot.steering_angle = 0.0
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", dist
robot.canproxy.go()
moving = True
if not robot.buttonGo:
print "STOP!"
break
robot.canproxy.stop_turn()
center(robot)
robot.laser.requestStop()
robot.gps.requestStop()
