naoqi_sonar.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (C) 2014 Aldebaran Robotics
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #     http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 #
00017 
00018 #import ROS dependencies
00019 import rospy
00020 from sensor_msgs.msg import Range
00021 
00022 #import NAO dependencies
00023 from naoqi_driver.naoqi_node import NaoqiNode
00024 
00025 class SonarSensor(object):
00026 
00027     # default values
00028     # NAO sonar specs
00029     # see https://community.aldebaran.com/doc/1-14/family/robots/sonar_robot.html
00030     SONAR_FREQ = 10
00031     SONAR_MIN_RANGE = 0.25
00032     SONAR_MAX_RANGE = 2.55
00033     SONAR_FOV = 0.523598776
00034 
00035     def __init__(self, memoryKey, frameID, rosTopic):
00036         self.memoryKey = memoryKey
00037         self.rosTopic = rosTopic
00038 
00039         self.msg = Range()
00040         self.msg.header.frame_id = frameID
00041         self.msg.min_range = self.SONAR_MIN_RANGE
00042         self.msg.max_range = self.SONAR_MAX_RANGE
00043         self.msg.field_of_view = self.SONAR_FOV
00044         self.msg.radiation_type = Range.ULTRASOUND
00045 
00046 
00047 class SonarPublisher(NaoqiNode):
00048 
00049     NAOQI_SONAR_SUB_NAME = 'ros_sonar_subsription'
00050 
00051     def __init__(self, param_sonar_rate="~sonar_rate", sonar_rate=10):
00052         NaoqiNode.__init__(self, "sonar_publisher")
00053         self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
00054         self.connectNaoQi()
00055 
00056         memory_key = rospy.get_param("~memory_key")
00057         frame_id = rospy.get_param("~frame_id")
00058         self.sonarSensor = SonarSensor( memory_key, frame_id, "sonar" )
00059         self.publisher = rospy.Publisher( self.sonarSensor.rosTopic, Range, queue_size=5)
00060 
00061     # (re-) connect to NaoQI:
00062     def connectNaoQi(self):
00063         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00064         self.memProxy = self.get_proxy("ALMemory")
00065         self.sonarProxy = self.get_proxy("ALSonar")
00066         if self.sonarProxy is None or self.memProxy is None:
00067             rospy.loginfo( 'sonar or memory proxy is invalid' )
00068             exit(1)
00069 
00070     # do it!
00071     def run(self):
00072         # start subscriber to sonar sensor
00073         self.sonarProxy.subscribe(self.NAOQI_SONAR_SUB_NAME)
00074 
00075         while self.is_looping():
00076             if (self.publisher.get_num_connections() > 0):
00077                 sonar = self.sonarSensor
00078                 sonar.msg.header.stamp = rospy.Time.now()
00079                 # fetch values
00080                 try:
00081                     sonar.msg.range = self.memProxy.getData(sonar.memoryKey)
00082                 except RuntimeError as e:
00083                     rospy.loginfo( 'key not found, correct robot ?', e )
00084                     break
00085     
00086                 # publish messages
00087                 self.publisher.publish(sonar.msg)
00088                 #sleep
00089                 self.sonarRate.sleep()
00090             
00091         #exit sonar subscription
00092         self.sonarProxy.unsubscribe(self.NAOQI_SONAR_SUB_NAME)


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 15:25:16