def __init__(self):
# Set some parameters
rospy.set_param("HALT", False)
rospy.set_param("WAIT", False)
rospy.set_param("HOLD", False)
# Subscribe to odometry
rospy.Subscriber('odom', Odometry, self.__parse_current_location)
# dispatch bumper control
self.bumper_control = BumperControl(dispatcher=self)
self.avoidance = Avoidance(dispatcher=self, is_navigation=True)
# wait until laser is ready
while not self.avoidance.is_laser_loaded():
pass
# clear the stdout and wait
self.shell = subprocess.Popen("clear", shell=True)
self.shell.wait()
print("Welcome")
while not rospy.is_shutdown():
# dispatch task planner
self.task_planner = TaskPlanner(dispatcher=self)
# dispatch navigation
self.navigation = Navigation(dispatcher=self)
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():
mapUsed = [] #store the mapsync obj of the maps to be used later.
interMapNodes = []
COM12 = MapSync()
COM12.loadLocation('1', '2')
COM22 = MapSync()
COM22.loadLocation('2', '2')
COM23 = MapSync()
COM23.loadLocation('2', '3')
allMaps = {'12' : COM12, '22' : COM22 , '23': COM23}
# voiceOutput = Voice()
### VOICE THREAD ###
voiceOutput = VoiceHandler()
voiceThread = threading.Thread(target = voiceOutput.voiceLoop)
voiceThread.start()
guiding = Guide(voiceOutput)
initialise.arduinoHandshake(voiceOutput) #add in the code whereby if handshake fails, try another few more times
initialise.calibrateStep(voiceOutput)
#userInput = initialise.getInitialInput(voiceOutput)
userInput = {'buildingstart': '1', 'levelstart': '2', 'start': '14', 'buildingend': '2', 'levelend': '2', 'end': '17'}
startKey = userInput.get('buildingstart') + userInput.get('levelstart')
endKey = userInput.get('buildingend') + userInput.get('levelend')
mapUsed.append(allMaps.get(startKey))
interMapNodes.append(userInput.get('start'))
if userInput.get('buildingstart') != userInput.get('buildingend'):
if userInput.get('levelstart') != userInput.get('levelend'): #diff floor diff build
for k, v in (mapUsed[0].mapConnection).iteritems():
if v.get('building') == '2' and v.get('level') == '2' :
interMapNodes.append(k)
interMapNodes.append(v.get('node'))
mapUsed.append(allMaps.get('22'))
for k, v in (mapUsed[1].mapConnection).iteritems():
if v.get('building') == userInput.get('buildingend') and v.get('level') == userInput.get('levelend') :
interMapNodes.append(k)
interMapNodes.append(v.get('node'))
mapUsed.append(allMaps.get(endKey))
else:
print "inside" #diff floor same level
for k, v in (mapUsed[0].mapConnection).iteritems():
if v.get('building') == userInput.get('buildingend') and v.get('level') == userInput.get('levelend') :
interMapNodes.append(k)
interMapNodes.append(v.get('node'))
mapUsed.append(allMaps.get(endKey))
else:
if userInput.get('levelstart') != userInput.get('levelend'):#same level diff build
for k, v in (mapUsed[0].mapConnection).iteritems():
if v.get('building') == userInput.get('buildingend') and v.get('level') == userInput.get('levelend') :
interMapNodes.append(k)
interMapNodes.append(v.get('node'))
mapUsed.append(allMaps.get(endKey))
interMapNodes.append(userInput.get('end'))
print "current interMap coor is " ,
print interMapNodes
print "mapUsed is "
for i in range(len(mapUsed)):
currmap = mapUsed[i]
apNodes = currmap.apNodes
# map north stored as anti clockwise
# previous calculation is based on rotating anti clockwise,
# current input is based on clockwise, hence need to offset
map_north = (abs(currmap.north-360))%360
mapNodes = currmap.mapNodes
navigate = Navigation(mapNodes, map_north, voiceOutput)
navigate.getRoute(interMapNodes[i*2], interMapNodes[i*2+1])
navigate.beginNavigation(apNodes)
if i != len(mapUsed)-1:
# voiceOutput.say('You have reach the end of a map, switching to a new map.',2)
voiceOutput.addToQueue('You have reach the end of a map, switching to a new map.', constants.HIGH_PRIORITY)
guiding.destinationReached()
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"
