naoqi_sonar.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # Copyright (C) 2014 Aldebaran Robotics
4 #
5 # Licensed under the Apache License, Version 2.0 (the "License");
6 # you may not use this file except in compliance with the License.
7 # You may obtain a copy of the License at
8 #
9 # http://www.apache.org/licenses/LICENSE-2.0
10 #
11 # Unless required by applicable law or agreed to in writing, software
12 # distributed under the License is distributed on an "AS IS" BASIS,
13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 # See the License for the specific language governing permissions and
15 # limitations under the License.
16 #
17 
18 #import ROS dependencies
19 import rospy
20 from sensor_msgs.msg import Range
21 
22 #import NAO dependencies
23 from naoqi_driver.naoqi_node import NaoqiNode
24 
25 class SonarSensor(object):
26 
27  # default values
28  # NAO sonar specs
29  # see https://community.aldebaran.com/doc/1-14/family/robots/sonar_robot.html
30  SONAR_FREQ = 10
31  SONAR_MIN_RANGE = 0.25
32  SONAR_MAX_RANGE = 2.55
33  SONAR_FOV = 1.05
34 
35  def __init__(self, memoryKey, frameID, rosTopic):
36  self.memoryKey = memoryKey
37  self.rosTopic = rosTopic
38 
39  self.msg = Range()
40  self.msg.header.frame_id = frameID
41  self.msg.min_range = self.SONAR_MIN_RANGE
42  self.msg.max_range = self.SONAR_MAX_RANGE
43  self.msg.field_of_view = self.SONAR_FOV
44  self.msg.radiation_type = Range.ULTRASOUND
45 
46 
48 
49  NAOQI_SONAR_SUB_NAME = 'ros_sonar_subsription'
50 
51  def __init__(self, param_sonar_rate="~sonar_rate", sonar_rate=10):
52  NaoqiNode.__init__(self, "sonar_publisher")
53  self.sonarRate = rospy.Rate(rospy.get_param(param_sonar_rate, sonar_rate))
54  self.connectNaoQi()
55 
56  memory_key = rospy.get_param("~memory_key")
57  frame_id = rospy.get_param("~frame_id")
58  self.sonarSensor = SonarSensor( memory_key, frame_id, "sonar" )
59  self.publisher = rospy.Publisher( self.sonarSensor.rosTopic, Range, queue_size=5)
60 
61  # (re-) connect to NaoQI:
62  def connectNaoQi(self):
63  rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
64  self.memProxy = self.get_proxy("ALMemory")
65  self.sonarProxy = self.get_proxy("ALSonar")
66  if self.sonarProxy is None or self.memProxy is None:
67  rospy.loginfo( 'sonar or memory proxy is invalid' )
68  exit(1)
69 
70  # do it!
71  def run(self):
72  # start subscriber to sonar sensor
73  self.sonarProxy.subscribe(self.NAOQI_SONAR_SUB_NAME)
74 
75  while self.is_looping():
76  if (self.publisher.get_num_connections() > 0):
77  sonar = self.sonarSensor
78  sonar.msg.header.stamp = rospy.Time.now()
79  # fetch values
80  try:
81  sonar.msg.range = self.memProxy.getData(sonar.memoryKey)
82  except RuntimeError as e:
83  rospy.loginfo( 'key not found, correct robot ?', e )
84  break
85 
86  # publish messages
87  self.publisher.publish(sonar.msg)
88 
89  #sleep
90  self.sonarRate.sleep()
91 
92  #exit sonar subscription
93  #self.sonarProxy.unsubscribe(self.NAOQI_SONAR_SUB_NAME)
def __init__(self, memoryKey, frameID, rosTopic)
Definition: naoqi_sonar.py:35
def __init__(self, param_sonar_rate="~sonar_rate", sonar_rate=10)
Definition: naoqi_sonar.py:51
def get_proxy(self, name, warn=True)


naoqi_sensors_py
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 Thu Jul 16 2020 03:18:33