Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import rospy
00019
00020
00021 from naoqi_driver.naoqi_node import NaoqiNode
00022
00023 class LoadingMap(NaoqiNode):
00024
00025 def __init__(self):
00026 NaoqiNode.__init__(self, 'loading_map')
00027 self.navigation = None
00028 self.connectNaoQi()
00029
00030
00031 def connectNaoQi(self):
00032 rospy.loginfo("Node Loading map at %s:%d", self.pip, self.pport)
00033
00034 self.navigation = self.get_proxy("ALNavigation")
00035 if self.navigation is None:
00036 rospy.logerr("Unable to reach ALNavigation.")
00037 exit(0)
00038
00039 version_array = self.get_proxy("ALSystem").systemVersion().split('.')
00040 if len(version_array) < 3:
00041 rospy.logerr("Unable to deduce the system version.")
00042 exit(0)
00043 version_tuple = (int(version_array[0]), int(version_array[1]), int(version_array[2]))
00044 min_version = (2, 5, 2)
00045 if version_tuple < min_version:
00046 rospy.logerr("Naoqi version " + str(min_version) +
00047 " required for localization. Yours is " + str(version_tuple))
00048 exit(0)
00049
00050 def loadMap(self, pathtomap):
00051 '''Loading the map
00052 '''
00053 self.navigation.stopLocalization()
00054 res = self.navigation.loadExploration(pathtomap)
00055 if res:
00056 rospy.loginfo("The map %s has been loaded", pathtomap)
00057 else:
00058 rospy.logwarn("The map %s cannot be loaded", pathtomap)
00059 return res
00060
00061 def relocalization(self, x, y, theta):
00062 '''Relocalization of the robot according to the position given as input
00063 Input : The robot's position (x, y, theta)
00064 '''
00065 self.navigation.relocalizeInMap([x ,y ,theta])
00066 self.navigation.startLocalization()
00067
00068
00069 if __name__ == '__main__':
00070 rospy.loginfo("LoadingMap based on Naoqi")
00071 pub = LoadingMap()
00072
00073
00074 raw_input("Move Pepper to the position that corresponds to\
00075 zero in the map and press enter")
00076 pathtomap = rospy.get_param('~path_to_map', "")
00077 if pub.loadMap(pathtomap):
00078 pub.relocalization(0,0,0)
00079
00080 exit(0)