def main():
print "initiating engine..."
seed = 9999
dequeueCount = 0
time_start = time.time()
targetProcess = "node server.js"
targetSignal = signal.SIGUSR1
serverMinTimeDelay = 5
pid = getpid(targetProcess)
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
trajectory = []
while time.time() - time_start < 600:
time_thisLoop = time.time()
if not q.empty():
while (not q.empty()):
slam.update(q.get())
dequeueCount = dequeueCount + 1
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get map from this batch
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes, 'raw', 'L', 0, 1)
image.save("../../webgui/images/" + "map" + ".png")
print "map created after {} scans".format(dequeueCount)
# Try to tell the server
if pid == False:
print "failure: no server pid"
pid = getpid(targetProcess)
else:
try:
os.kill(int(pid),targetSignal)
print "success: signal sent to server"
except OSError:
print "error: whoops, just lost the pid"
pid = getpid(targetProcess)
# give the server at least serverMinTimeDelay seconds to catch up
while time.time() - time_thisLoop < serverMinTimeDelay:
()
def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Allocate byte array to receive map updates
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Load the data from the file
timestamps, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display, named by dataset
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, dataset)
# Pose will be modified in our threaded code
pose = [0, 0, 0]
# Launch the data-collection / update thread
thread = Thread(target=threadfunc,
args=(robot, slam, timestamps, lidars,
odometries if use_odometry else None, mapbytes,
pose))
thread.daemon = True
thread.start()
# Loop forever,displaying current map and pose
while True:
# Display map and robot pose
display.displayMap(mapbytes)
display.setPose(*pose)
# Refresh the display, exiting gracefully if user closes it
if not display.refresh():
exit(0)
def main():
seed = 9999
runCount = 0
dequeueCount = 0
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
trajectory = []
while dequeueCount < 1000:
time.sleep(10)
if q.empty() == False:
while (q.empty() == False):
slam.update(q.get())
#print "%i" %dequeueCount
dequeueCount = dequeueCount + 1
x_mm, y_mm, theta_degrees = slam.getpos()
trajectory.append((x_mm, y_mm))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS),
mapbytes, 'raw', 'L', 0, 1)
#image.save('map%i.png' %runCount)
#image.save("/home/card/webgui/images/" + "map" + str(dequeueCount) + ".png")
image.save("/home/card/webgui/images/" + "map" + ".png")
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file, ignoring timestamps
_, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Pickle the map to a file
pklname = dataset + '.map'
print('Writing map to file ' + pklname)
pickle.dump(mapbytes, open(pklname, 'wb'))
def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <output_dir> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 output 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3])
output_filename = argv[4]
draw_trajectory = True if int(argv[5]) else False
print("dataset: " + dataset)
print("use_odometry: " + str(use_odometry))
print("seed: " + str(seed))
print("output_filename: " + output_filename)
print("draw_trajectory: " + str(draw_trajectory))
# Load the data from the file, ignoring timestamps
_, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Robot() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(Laser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(Laser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to pose change (dxy_mm, dtheta_degrees, dt_seconds)
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
if(draw_trajectory):
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
# Save map and trajectory as PGM file
pgm_save(output_filename, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
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():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
lidarobj = Laser(360, 12, 360, 8000)
# Create a CoreSLAM object with laser params and robot object
slam = RMHC_SLAM(lidarobj, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Start with an empty trajectory of positions
trajectory = []
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
suffix = 1
while (True):
if (use_odometry):
mutex.acquire()
mainLCMQ = lcmQ
mainODOQ = odoQ
# Clear Queues once copied from thread into main for next batch of data
lcmQ.queue.clear()
odoQ.queue.clear()
mutex.release()
velocities = robot.computePoseChange(mainODOQ.get())
slam.update(mainLCMQ.get(), velocities)
x_mm, y_mm, theta_degrees = slam.getpos()
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
trajectory.append((y_pix, x_pix))
slam.getmap(mapbytes)
trajLen = len(trajectory)
for i in range(trajLen):
if (i == (trajLen - 1)):
mapbytes[trajectory[i][0] * MAP_SIZE_PIXELS +
trajectory[i][1]] = 0
else:
mapbytes[trajectory[i][0] * MAP_SIZE_PIXELS +
trajectory[i][1]] = 120
filename = dataset + str(suffix)
pgm_save('%s.pgm' % filename, mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
suffix += 1
if (keyPressed == 's'):
#Wrap up last map using leftover data
pgm_save('%s.pgm' % filename, mapbytes,
(MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
'''
This will take all the maps generated and place them into pgmbagfolder
For this to work, make sure your destination directory has a folder called pgmbagfolder
Change the directory:
/home/Shaurya98/rplidar_workspace/src/mapping/BreezySLAM/examples
and
/home/Shaurya98/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder
With your own destination directory. It it recommended to put pgmbagfolder under the examples
directory
'''
os.chdir(
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder"
)
for pgm_file in glob.iglob('*.pgm'):
os.remove(pgm_file)
print("\nEmptied pgmbagfolder")
os.chdir(
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples"
)
for pgm_file in glob.iglob('*.pgm'):
shutil.copy2(
pgm_file,
"/home/pi/rplidar_workspace/src/mapping/BreezySLAM/examples/pgmbagfolder"
)
os.remove(pgm_file)
print("\nFiles recorded and sent to pgmbagfolder")
#Terminate threads before exiting main()
thread1.join()
thread2.join()
thread3.join()
break
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():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file
lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes,
'raw', 'L', 0, 1)
image.save('%s.png' % dataset)
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"