i += 1
if i >= len(data):
break
if len(tmp) > 0:
print(i, tmp)
ret.append( (i + len(tmp)/2, 0.002*sum(tmp)/len(tmp)) )
return ret
if __name__ == "__main__":
if len(sys.argv) < 2:
print(__doc__)
sys.exit(2)
assert 'meta_' in sys.argv[1], sys.argv[1]
metalog = MetaLog(filename=sys.argv[1])
sensor = Velodyne(metalog=metalog)
prev = None
for ii in range(20000):
sensor.update()
curr = sensor.scan_index, sensor.safe_dist
if prev != curr:
if sensor.scan_index % 1 == 0:
print('-----', sensor.scan_index, '-----')
data = detect_columns(sensor.dist)
pose = analyse_pose(None, data)
if pose is not None:
pose, dist = pose
print_pose(pose)
print(dist)
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)
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 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()