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 nao_driver import NaoNode
00024 from nao_driver.boost.octomap_python import octomap_str_to_tuple
00025 
00026 class NaoOctomap(NaoNode):
00027     def __init__(self):
00028         NaoNode.__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         proxy._setObstacleModeForSafety(1)
00039 
00040         # Create ROS publisher
00041         self.pub = rospy.Publisher("octomap", Octomap, latch = True, queue_size=1)
00042 
00043         self.fps = 1
00044 
00045         rospy.loginfo("nao_octomap initialized")
00046 
00047     def run(self):
00048         r = rospy.Rate(self.fps)
00049         octomap = Octomap()
00050         octomap.header.frame_id = '/odom'
00051 
00052         while self.is_looping():
00053             octomap_bin = self.get_proxy("ALNavigation")._get3DMap()
00054             octomap.binary, octomap.id, octomap.resolution, octomap.data = octomap_str_to_tuple(octomap_bin)
00055 
00056             octomap.header.stamp = rospy.Time.now()
00057 
00058             self.pub.publish(octomap)
00059 
00060             r.sleep()
00061 
00062 if __name__ == '__main__':
00063     nao_octomap = NaoOctomap()
00064     nao_octomap.start()
00065     rospy.spin()


nao_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 Mon Oct 6 2014 02:40:25