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"