def main(log, readlog, only_odometry, sensorFile, odomFile, resultname, mapconfig):
initialized = False
sensor = None
navigation = None
robot = None
server = None
try:
#Initialize Sensor
if(readlog):
sensor = FileXTION(sensorFile)
else:
sensor = XTION(log)
#Initialize Slam
if(only_odometry):
slam = Deterministic_SLAM(sensor, mapconfig.SIZE_PIXELS, mapconfig.SIZE_METERS)
else:
slam = My_SLAM(sensor, mapconfig.SIZE_PIXELS, mapconfig.SIZE_METERS, random_seed = SEED)
#Initialize Robot
if(readlog):
robot = FileDrone(odomFile)
else:
robot = NetworkVehicle(log)
robot.initialize()
#Open Controll and Map Server
server = Server(slam, mapconfig.SIZE_PIXELS, robot)
server.start()
#Initialize Navigation
if(not readlog):
navigation = Navigation(slam, mapconfig, robot.getSize(), RELEVANT_LIDARS, SECURITY_DIST_MM, Commands)
navigation.start()
#Monitors
scanno = 0
dist = 0
timePast = 0
trajectory = []
start_sec = time()
#Make initial scan
scan = sensor.scan()
initialized = True
#Main loop
while(True):
scanno += 1
#get command
if(readlog):
command = None
else:
command = navigation.update(scan)
if(command == None):
print "Navigation terminated."
break
#send command and get odometry
velocities = robot.move(command)
#check if velocities are valid
if(velocities == None):
print "Robot terminated."
break
#update monitors
dist += velocities[0]
timePast += velocities[2]
#get scan
scan = sensor.scan()
#check if scan is valid
if(len(scan)<=0):
print "Sensor terminated."
break
#Update SLAM
slam.update(scan, velocities)
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
except KeyboardInterrupt:
print "Program stoped!"
finally:
print "Shutting down."
if(sensor != None): sensor.shutdown()
if(navigation != None): navigation.stop()
if(robot != None): robot.shutdown()
if(server != None): server.close()
if(initialized):
#Print results
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' % (scanno, elapsed_sec, scanno/elapsed_sec))
print ('Distance traveled:%f mm in %fs' % (dist, timePast))
#generate map
mapbytes = createMap(slam, trajectory, mapconfig)
# Save map and trajectory as png file
mapconfig.safeaspng(mapbytes, resultname)
def main(log, readlog, only_odometry, sensorFile, odomFile, resultname,
mapconfig):
initialized = False
sensor = None
navigation = None
robot = None
server = None
try:
#Initialize Sensor
if (readlog):
sensor = FileXTION(sensorFile)
else:
sensor = XTION(log)
#Initialize Slam
if (only_odometry):
slam = Deterministic_SLAM(sensor, mapconfig.SIZE_PIXELS,
mapconfig.SIZE_METERS)
else:
slam = My_SLAM(sensor,
mapconfig.SIZE_PIXELS,
mapconfig.SIZE_METERS,
random_seed=SEED)
#Initialize Robot
if (readlog):
robot = FileDrone(odomFile)
else:
robot = NetworkVehicle(log)
robot.initialize()
#Open Controll and Map Server
server = Server(slam, mapconfig.SIZE_PIXELS, robot)
server.start()
#Initialize Navigation
if (not readlog):
navigation = Navigation(slam, mapconfig, robot.getSize(),
RELEVANT_LIDARS, SECURITY_DIST_MM,
Commands)
navigation.start()
#Monitors
scanno = 0
dist = 0
timePast = 0
trajectory = []
start_sec = time()
#Make initial scan
scan = sensor.scan()
initialized = True
#Main loop
while (True):
scanno += 1
#get command
if (readlog):
command = None
else:
command = navigation.update(scan)
if (command == None):
print "Navigation terminated."
break
#send command and get odometry
velocities = robot.move(command)
#check if velocities are valid
if (velocities == None):
print "Robot terminated."
break
#update monitors
dist += velocities[0]
timePast += velocities[2]
#get scan
scan = sensor.scan()
#check if scan is valid
if (len(scan) <= 0):
print "Sensor terminated."
break
#Update SLAM
slam.update(scan, velocities)
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
except KeyboardInterrupt:
print "Program stoped!"
finally:
print "Shutting down."
if (sensor != None): sensor.shutdown()
if (navigation != None): navigation.stop()
if (robot != None): robot.shutdown()
if (server != None): server.close()
if (initialized):
#Print results
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(scanno, elapsed_sec, scanno / elapsed_sec))
print('Distance traveled:%f mm in %fs' % (dist, timePast))
#generate map
mapbytes = createMap(slam, trajectory, mapconfig)
# Save map and trajectory as png file
mapconfig.safeaspng(mapbytes, resultname)
def main(g=0.4, h=0.4):
filename = 'map_%f_%f' % (g, h)
"""
if(use_odometry):
filename += 'withodometry_'
if(readlog):
filename += 'fromlog_'
if(seed==0):
filename += 'deterministic'
else:
filename += ('rmhc_seed' + str(seed))
"""
#initialize the asus xtion as sensor
if (readlog):
sensor = FileXTION("log")
else:
sensor = XTION() #NetworkSensor() #
# Create a CoreSLAM object with laser params and optional robot object
slam = My_SLAM(sensor, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed, g=g, h=h) \
if seed \
else Deterministic_SLAM(sensor, MAP_SIZE_PIXELS, MAP_SIZE_METERS)
robot = None
#initialiye robot
if (use_odometry):
navigation = Navigation(slam, MapConfig(), ROBOT_SIZE_METERS, 50, 800,
Commands)
if (readlog):
robot = FileDrone("odometry")
else:
robot = NetworkVehicle() #Drone()
robot.initialize()
if (stream):
server = Server(slam, MAP_SIZE_PIXELS, robot)
server.start()
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop
atexit.register(set_normal_term)
set_curses_term()
scanno = 0
dist = 0
zeit = 0
if (use_odometry): navigation.start()
##make initial scan
scan = sensor.scan()
while (True):
scanno += 1
if use_odometry:
##navigaiton
command = navigation.update(scan)
#print command
##odometry
velocities = robot.move(command)
dist += velocities[0]
zeit += velocities[2]
print velocities
##lidar
scan = sensor.scan()
if (len(scan) <= 0):
print 'Reader error or end of file.'
break
# Update SLAM with lidar and velocities
slam.update(scan, velocities)
else:
scan = sensor.scan()
if (len(scan) <= 0):
print 'Reader error or end of file.'
break
# Update SLAM with lidar alone
slam.update(scan)
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
if kbhit():
break
if (use_odometry):
robot.shutdown()
navigation.stop()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(scanno, elapsed_sec, scanno / elapsed_sec))
print('dist traveled:%f mm in %fs' % (dist, zeit))
mapbytes = createMap(slam, trajectory)
# Save map and trajectory as PGM file
pgm_save(filename, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
image = cv2.imread(filename, 0)
print "Accessing the image.. again. So dirty."
print "Saving as .png: ..."
cv2.imwrite("test.png", image)
if (stream):
server.close()
print "done"
