naoqi_microphone.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 from collections import defaultdict
19 import rospy
20 
21 from naoqi_driver.naoqi_node import NaoqiNode
22 
23 from dynamic_reconfigure.server import Server
24 from naoqi_sensors_py.cfg import NaoqiMicrophoneConfig
25 from naoqi_bridge_msgs.msg import AudioBuffer
26 
27 from naoqi import ALModule, ALBroker, ALProxy
28 
29 class NaoqiMic (ALModule, NaoqiNode):
30  def __init__(self, name):
31  NaoqiNode.__init__(self, name)
32  self.myBroker = ALBroker("pythonBroker", "0.0.0.0", 0, self.pip, self.pport)
33  ALModule.__init__(self, name)
34 
35  self.isSubscribed = False
36  self.microVersion = 0
37 
38  # Create a proxy to ALAudioDevice
39  try:
40  self.audioProxy = self.get_proxy("ALAudioDevice")
41  except Exception, e:
42  rospy.logerr("Error when creating proxy on ALAudioDevice:")
43  rospy.logerr(str(e))
44  exit(1)
45 
46  try:
47  self.robotProxy = self.get_proxy("ALRobotModel")
48  self.microVersion = self.robotProxy._getMicrophoneConfig()
49  except Exception, e:
50  rospy.logwarn("Could not retrieve microphone version:")
51  rospy.logwarn(str(e))
52  rospy.logwarn("Microphone channel map might not be accurate.")
53 
54  def returnNone():
55  return None
56  self.config = defaultdict(returnNone)
57 
58  # ROS publishers
59  self.pub_audio_ = rospy.Publisher('~audio_raw', AudioBuffer)
60 
61  # initialize the parameter server
62  self.srv = Server(NaoqiMicrophoneConfig, self.reconfigure)
63 
64  def reconfigure( self, new_config, level ):
65  """
66  Reconfigure the microphones
67  """
68  rospy.loginfo('reconfigure changed')
69  if self.pub_audio_.get_num_connections() == 0:
70  rospy.loginfo('Changes recorded but not applied as nobody is subscribed to the ROS topics.')
71  self.config.update(new_config)
72  return self.config
73 
74  # check if we are already subscribed
75  if not self.isSubscribed:
76  rospy.loginfo('subscribed to audio proxy, since this is the first listener')
77  self.audioProxy.setClientPreferences(self.getName(), new_config['frequency'], 0, 0)
78  self.audioProxy.subscribe(self.getName())
79  self.isSubscribed = True
80 
81  self.config.update(new_config)
82 
83  return self.config
84 
85  def run(self):
86  r=rospy.Rate(2)
87  while self.is_looping():
88  if self.pub_audio_.get_num_connections() == 0 and self.isSubscribed:
89  rospy.loginfo('Unsubscribing from audio bridge as nobody listens to the topics.')
90  self.release()
91 
92  elif self.pub_audio_.get_num_connections() > 0 and not self.isSubscribed:
93  self.reconfigure(self.config, 0)
94 
95  r.sleep()
96 
97  if self.isSubscribed:
98  self.release()
99  self.myBroker.shutdown()
100 
101  def release(self):
102  self.audioProxy.unsubscribe(self.name)
103  self.isSubscribed=False
104 
105  def processRemote(self, nbOfInputChannels, fNbOfInputSamples, timeStamp, inputBuff):
106  audio_msg = AudioBuffer()
107 
108  # Deal with the sound
109  # get data directly with the _getInputBuffer() function because inputBuff is corrupted in python
110  mictmp = []
111  for i in range (0,len(inputBuff)/2) :
112  mictmp.append(ord(inputBuff[2*i])+ord(inputBuff[2*i+1])*256)
113 
114  # convert 16 bit samples to signed 16 bits samples
115  for i in range (0,len(mictmp)) :
116  if mictmp[i]>=32768 :
117  mictmp[i]=mictmp[i]-65536
118 
119  if self.config['use_ros_time']:
120  audio_msg.header.stamp = rospy.Time.now()
121  else:
122  audio_msg.header.stamp = rospy.Time(timeStamp)
123 
124  audio_msg.frequency = self.config['frequency']
125  if self.microVersion == 0:
126  channels = [0,2,1,4]
127  else:
128  channels = [3,5,0,2]
129 
130  audio_msg.channelMap = channels
131 
132  audio_msg.data = mictmp
133  self.pub_audio_.publish(audio_msg)
def reconfigure(self, new_config, level)
def processRemote(self, nbOfInputChannels, fNbOfInputSamples, timeStamp, inputBuff)
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