loading_map.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Copyright (C) 2017 SoftBank Robotics Europe
00003 #
00004 # Licensed under the Apache License, Version 2.0 (the "License");
00005 # you may not use this file except in compliance with the License.
00006 # You may obtain a copy of the License at
00007 #
00008 #     http://www.apache.org/licenses/LICENSE-2.0
00009 #
00010 # Unless required by applicable law or agreed to in writing, software
00011 # distributed under the License is distributed on an "AS IS" BASIS,
00012 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013 # See the License for the specific language governing permissions and
00014 # limitations under the License.
00015 #
00016 
00017 #import ROS dependencies
00018 import rospy
00019 
00020 #import NAO dependencies
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     # (re-) connect to NaoQI:
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     #load the map and relocalize
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)


python
Author(s):
autogenerated on Sat Jun 8 2019 20:30:20