Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 import rospy
00020 from sensor_msgs.msg import Range
00021
00022
00023 from naoqi_driver.naoqi_node import NaoqiNode
00024
00025 class SonarSensor(object):
00026
00027
00028
00029
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
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
00071 def run(self):
00072
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
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
00087 self.publisher.publish(sonar.msg)
00088
00089 self.sonarRate.sleep()
00090
00091
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