ros_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 nao_driver import NaoNode
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(NaoNode):
00048 
00049     NAOQI_SONAR_SUB_NAME = 'ros_sonar_subsription'
00050 
00051     def __init__(self, sonarSensorList, param_sonar_rate="~sonar_rate", sonar_rate=40):
00052         NaoNode.__init__(self, "sonar_publisher")
00053         self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
00054         self.connectNaoQi()
00055 
00056         self.sonarSensorList = sonarSensorList
00057         self.publisherList = []
00058         for sonarSensor in sonarSensorList:
00059             self.publisherList.append(
00060                         rospy.Publisher(sonarSensor.rosTopic, Range, queue_size=5))
00061 
00062     # (re-) connect to NaoQI:
00063     def connectNaoQi(self):
00064         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00065         self.sonarProxy = self.get_proxy("ALSonar")
00066         self.memProxy = self.get_proxy("ALMemory")
00067         if self.sonarProxy is None or self.memProxy is None:
00068             #self.sonarProxy.unsubscribe(NAO_SONAR_SUB_NAME)
00069             exit(1)
00070 
00071     # do it!
00072     def run(self):
00073         # start subscriber to sonar sensor
00074         self.sonarProxy.subscribe(self.NAOQI_SONAR_SUB_NAME)
00075 
00076         while self.is_looping():
00077             for i in range(len(self.sonarSensorList)):
00078                 sonar = self.sonarSensorList[i]
00079                 sonar.msg.header.stamp = rospy.Time.now()
00080                 # fetch values
00081                 try:
00082                     sonar.msg.range = self.memProxy.getData(sonar.memoryKey)
00083                 except RuntimeError as e:
00084                     print 'key not found, correct robot ?', e
00085                     break
00086 
00087                 # publish messages
00088                 self.publisherList[i].publish(sonar.msg)
00089                 #sleep
00090                 self.sonarRate.sleep()
00091 
00092         #exit sonar subscription
00093         self.sonarProxy.unsubscribe(self.NAOQI_SONAR_SUB_NAME)


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 Sun Nov 2 2014 11:27:42