def driver_self_test(driver, 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
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
go_straight(robot, distance=1.0, speed=0.3, with_stop=True)
for i in range(3):
turn(robot, math.radians(-45), radius=2.6, speed=0.5)
turn(robot, math.radians(-45), radius=2.6, speed=-0.5)
robot.canproxy.stop_turn()
robot.wait(3.0)
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 create_robot(metalog, conf):
assert metalog is not None
assert conf is not None # config is required!
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)
loc = SimpleOdometry.from_dict(conf.data.get('localization'))
robot = JohnDeere(can=can, localization=loc, config=conf.data.get('johndeere'))
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
return robot
def driver_self_test(driver, 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
robot.canproxy.stop()
robot.canproxy.set_turn_raw(0)
go_straight(robot, distance=1.0, speed=0.3, with_stop=True)
for i in xrange(3):
turn(robot, math.radians(-45), radius=2.6, speed=0.5)
turn(robot, math.radians(-45), radius=2.6, speed=-0.5)
robot.canproxy.stop_turn()
robot.wait(3.0)
def create_robot(metalog, conf):
assert metalog is not None
assert conf is not None # config is required!
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)
loc = SimpleOdometry.from_dict(conf.data.get('localization'))
robot = JohnDeere(can=can,
localization=loc,
config=conf.data.get('johndeere'))
robot.UPDATE_TIME_FREQUENCY = 20.0 # TODO change internal and integrate setup
return 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)
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)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print(velodyne_log_name)
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist,
velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start(
) # the data source is active, so it is necessary to read-out data
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()
if robot.gps_data != prev_gps:
print(robot.time, robot.gas, robot.gps_data, robot.velodyne_data)
prev_gps = robot.gps_data
dist = None
if robot.velodyne_data is not None:
dist_index = None
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print("!!! STOP !!! -", robot.velodyne_data)
#center(robot)
robot.canproxy.stop()
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
arr = robot.velodyne_data[1]
num = len(arr)
left, right = min(arr[:num / 2]), min(arr[num / 2:])
print("DECIDE", left, right, robot.velodyne_data)
if left <= right:
robot.canproxy.set_turn_raw(-100)
robot.steering_angle = math.radians(
-30) # TODO replace by autodetect
else:
robot.canproxy.set_turn_raw(100)
robot.steering_angle = math.radians(
30) # TODO replace by autodetect
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
robot.canproxy.set_turn_raw(0)
robot.steering_angle = 0.0 # TODO replace by autodetect
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print("GO", robot.velodyne_data)
#go(robot)
robot.canproxy.go()
moving = True
if not robot.buttonGo:
print("STOP!")
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.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()
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 ver0(metalog, waypoints=None):
assert metalog is not None
assert waypoints is not None # for simplicity (first is start)
conv = Convertor(refPoint = waypoints[0])
waypoints = waypoints[1:-1] # remove start/finish
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
# mount_sensor(GPS, robot, metalog)
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)
# mount_sensor(VelodyneThread, robot, metalog)
velodyne_log_name = metalog.getLog('velodyne_dist')
print velodyne_log_name
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start() # the data source is active, so it is necessary to read-out data
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: # RO timelimit 30 minutes
robot.update()
if robot.gps_data != prev_gps:
print robot.time, robot.gas, robot.gps_data, robot.velodyne_data
prev_gps = robot.gps_data
if robot.gps_data is not None:
dist_arr = [distance( conv.geo2planar((robot.gps_data[1], robot.gps_data[0])),
conv.geo2planar(destination) ) for destination in waypoints]
dist = min(dist_arr)
print "DIST-GPS", dist
if prev_destination_dist is not None:
if prev_destination_dist < dist and dist < 10.0:
robot.drop_ball = True
# remove nearest
i = dist_arr.index(dist) # ugly, but ...
print "INDEX", i
del waypoints[i]
center(robot)
moving = False
robot.wait(1.0)
robot.drop_ball = False
robot.wait(3.0)
dist = None
prev_destination_dist = dist
dist = None
if robot.velodyne_data is not None:
index, dist = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!! -", robot.velodyne_data
center(robot)
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
arr = robot.velodyne_data[1]
num = len(arr)
left, right = min(arr[:num/2]), min(arr[num/2:])
print "DECIDE", left, right, robot.velodyne_data
if left <= right:
robot.pulse_right(RIGHT_TURN_TIME)
robot.steering_angle = math.radians(-30) # TODO replace by autodetect
else:
robot.pulse_left(LEFT_TURN_TIME)
robot.steering_angle = math.radians(30) # TODO replace by autodetect
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
if robot.steering_angle < 0:
robot.pulse_left(LEFT_TURN_TIME)
else:
robot.pulse_right(RIGHT_TURN_TIME)
robot.steering_angle = 0.0 # TODO replace by autodetect
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", robot.velodyne_data
go(robot)
moving = True
if not robot.buttonGo:
print "STOP!"
break
center(robot)
robot.velodyne.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,
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 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 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 followme(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)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print velodyne_log_name
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist,
velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start(
) # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
target_detected = 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()
if robot.gps_data != prev_gps:
if robot.velodyne_data is not None:
print(robot.velodyne_data[-1], robot.canproxy.wheel_angle_raw,
robot.canproxy.desired_wheel_angle_raw)
prev_gps = robot.gps_data
dist, dist_index = None, None
if robot.velodyne_data is not None:
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if dist_index is not None:
target_index, target_dist = dist_index
robot.canproxy.set_turn_raw(int((-100 / 45.) * target_index))
target_detected = target_dist < TARGET_DISTANCE
else:
dist_index = False
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP or not target_detected:
print "!!! STOP !!! -", robot.velodyne_data
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", target_detected, robot.velodyne_data
if target_detected:
robot.canproxy.go_slowly()
moving = True
if not robot.buttonGo:
print "STOP!"
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.requestStop()
robot.gps.requestStop()
def followme(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)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print(velodyne_log_name)
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start() # the data source is active, so it is necessary to read-out data
center(robot)
wait_for_start(robot)
moving = False
target_detected = 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()
if robot.gps_data != prev_gps:
if robot.velodyne_data is not None:
print((robot.velodyne_data[-1], robot.canproxy.wheel_angle_raw,
robot.canproxy.desired_wheel_angle_raw))
prev_gps = robot.gps_data
dist, dist_index = None, None
if robot.velodyne_data is not None:
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if dist_index is not None:
target_index, target_dist = dist_index
robot.canproxy.set_turn_raw(int((-100/45.)*target_index))
target_detected = target_dist < TARGET_DISTANCE
else:
dist_index = False
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP or not target_detected:
print("!!! STOP !!! -", robot.velodyne_data)
robot.canproxy.stop()
moving = False
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print("GO", target_detected, robot.velodyne_data)
if target_detected:
robot.canproxy.go_slowly()
moving = True
if not robot.buttonGo:
print("STOP!")
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.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()
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)
# Velodyne
velodyne_log_name = metalog.getLog('velodyne_dist')
print velodyne_log_name
sensor = Velodyne(metalog=metalog)
if metalog.replay:
robot.velodyne = DummySensor()
function = SourceLogger(None, velodyne_log_name).get
else:
robot.velodyne = VelodyneThread(sensor)
function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
robot.velodyne_data = None
robot.register_data_source('velodyne', function, velodyne_data_extension)
robot.gps.start() # ASAP so we get GPS fix
robot.velodyne.start() # the data source is active, so it is necessary to read-out data
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()
if robot.gps_data != prev_gps:
print robot.time, robot.gas, robot.gps_data, robot.velodyne_data
prev_gps = robot.gps_data
dist = None
if robot.velodyne_data is not None:
dist_index = None
if len(robot.velodyne_data) == 2:
index, dist = robot.velodyne_data
else:
index, dist, dist_index = robot.velodyne_data
if dist is not None:
dist = min(dist) # currently supported tupple of readings
if moving:
if dist is None or dist < SAFE_DISTANCE_STOP:
print "!!! STOP !!! -", robot.velodyne_data
#center(robot)
robot.canproxy.stop()
moving = False
elif dist < TURN_DISTANCE:
if abs(robot.steering_angle) < STRAIGHT_EPS:
arr = robot.velodyne_data[1]
num = len(arr)
left, right = min(arr[:num/2]), min(arr[num/2:])
print "DECIDE", left, right, robot.velodyne_data
if left <= right:
robot.canproxy.set_turn_raw(-100)
robot.steering_angle = math.radians(-30) # TODO replace by autodetect
else:
robot.canproxy.set_turn_raw(100)
robot.steering_angle = math.radians(30) # TODO replace by autodetect
elif dist > NO_TURN_DISTANCE:
if abs(robot.steering_angle) > STRAIGHT_EPS:
robot.canproxy.set_turn_raw(0)
robot.steering_angle = 0.0 # TODO replace by autodetect
else: # not moving
if dist is not None and dist > SAFE_DISTANCE_GO:
print "GO", robot.velodyne_data
#go(robot)
robot.canproxy.go()
moving = True
if not robot.buttonGo:
print "STOP!"
break
robot.canproxy.stop_turn()
center(robot)
robot.velodyne.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, 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)
