octomap.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Copyright (C) 2014 Aldebaran Robotics
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 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.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         # Create ROS publisher
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
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 Fri Jul 3 2015 12:51:48