Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 from distutils.version import LooseVersion
00018
00019 import rospy
00020
00021 from octomap_msgs.msg import Octomap
00022
00023 from naoqi_driver.naoqi_node import NaoqiNode
00024 from naoqi_sensors_py.boost.octomap_python import octomap_str_to_tuple
00025
00026 class NaoqiOctomap(NaoqiNode):
00027 def __init__(self):
00028 NaoqiNode.__init__(self, 'nao_octomap')
00029
00030 if self.get_version() < LooseVersion('2.0'):
00031 rospy.loginfo('NAOqi version < 2.0, Octomap is not used')
00032 exit(0)
00033
00034 proxy = self.get_proxy("ALNavigation")
00035 if proxy is None:
00036 rospy.loginfo('Could not get access to the ALNavigation proxy')
00037 exit(1)
00038
00039
00040 self.pub = rospy.Publisher("octomap", Octomap, latch = True, queue_size=1)
00041
00042 self.fps = 1
00043
00044 rospy.loginfo("nao_octomap initialized")
00045
00046 def run(self):
00047 r = rospy.Rate(self.fps)
00048 octomap = Octomap()
00049 octomap.header.frame_id = '/odom'
00050
00051 while self.is_looping():
00052 if self.pub.get_num_connections() > 0:
00053 octomap_bin = self.get_proxy("ALNavigation")._get3DMap()
00054 if len(octomap_bin) > 129:
00055 octomap.binary, octomap.id, octomap.resolution, octomap.data = octomap_str_to_tuple(octomap_bin)
00056 if len(octomap.data) > 0:
00057 octomap.header.stamp = rospy.Time.now()
00058 self.pub.publish(octomap)
00059
00060 r.sleep()
00061
00062 if __name__ == '__main__':
00063 nao_octomap = NaoqiOctomap()
00064 nao_octomap.start()
00065 rospy.spin()
naoqi_sensors_py
Author(s): Séverin Lemaignan, Vincent Rabaud, Karsten Knese, Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy, Daniel Maier
autogenerated on Wed Aug 16 2017 02:28:12