naoqi_microphone.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 from collections import defaultdict
00019 import rospy
00020 
00021 from naoqi_driver.naoqi_node import NaoqiNode
00022 
00023 from dynamic_reconfigure.server import Server
00024 from naoqi_sensors.cfg import NaoqiMicrophoneConfig
00025 from naoqi_msgs.msg import AudioBuffer
00026 
00027 from naoqi import ALModule, ALBroker, ALProxy
00028 
00029 class NaoqiMic (ALModule, NaoqiNode):
00030     def __init__(self, name):
00031         NaoqiNode.__init__(self, name)
00032         self.myBroker = ALBroker("pythonBroker", "0.0.0.0", 0, self.pip, self.pport)
00033         ALModule.__init__(self, name)
00034 
00035         self.isSubscribed = False
00036         self.microVersion = 0
00037 
00038         # Create a proxy to ALAudioDevice
00039         try:
00040             self.audioProxy = self.get_proxy("ALAudioDevice")
00041         except Exception, e:
00042             rospy.logerr("Error when creating proxy on ALAudioDevice:")
00043             rospy.logerr(str(e))
00044             exit(1)
00045 
00046         try:
00047             self.robotProxy = self.get_proxy("ALRobotModel")
00048             self.microVersion = self.robotProxy._getMicrophoneConfig()
00049         except Exception, e:
00050             rospy.logwarn("Could not retrieve microphone version:")
00051             rospy.logwarn(str(e))
00052             rospy.logwarn("Microphone channel map might not be accurate.")
00053 
00054         def returnNone():
00055             return None
00056         self.config = defaultdict(returnNone)
00057 
00058         # ROS publishers
00059         self.pub_audio_ = rospy.Publisher('~audio_raw', AudioBuffer)
00060 
00061         # initialize the parameter server
00062         self.srv = Server(NaoqiMicrophoneConfig, self.reconfigure)
00063 
00064     def reconfigure( self, new_config, level ):
00065         """
00066         Reconfigure the microphones
00067         """
00068         rospy.loginfo('reconfigure changed')
00069         if self.pub_audio_.get_num_connections() == 0:
00070            rospy.loginfo('Changes recorded but not applied as nobody is subscribed to the ROS topics.')
00071            self.config.update(new_config)
00072            return self.config
00073 
00074         # check if we are already subscribed
00075         if not self.isSubscribed:
00076             rospy.loginfo('subscribed to audio proxy, since this is the first listener')
00077             self.audioProxy.setClientPreferences(self.getName(), new_config['frequency'], 0, 0)
00078             self.audioProxy.subscribe(self.getName())
00079             self.isSubscribed = True
00080 
00081         self.config.update(new_config)
00082 
00083         return self.config
00084 
00085     def run(self):
00086         r=rospy.Rate(2)
00087         while self.is_looping():
00088             if self.pub_audio_.get_num_connections() == 0:
00089                 if self.isSubscribed:
00090                     rospy.loginfo('Unsubscribing from audio bridge as nobody listens to the topics.')
00091                     self.release()
00092                 continue
00093 
00094             if not self.isSubscribed:
00095                 self.reconfigure(self.config, 0)
00096 
00097             r.sleep()
00098 
00099         if self.isSubscribed:
00100             self.release()
00101         self.myBroker.shutdown()
00102 
00103     def release(self):
00104         self.audioProxy.unsubscribe(self.name)
00105         self.isSubscribed=False
00106 
00107     def processRemote(self, nbOfInputChannels, fNbOfInputSamples, timeStamp, inputBuff):
00108         audio_msg = AudioBuffer()
00109 
00110         # Deal with the sound
00111         # get data directly with the _getInputBuffer() function because inputBuff is corrupted in python
00112         mictmp = []
00113         for i in range (0,len(inputBuff)/2) :
00114             mictmp.append(ord(inputBuff[2*i])+ord(inputBuff[2*i+1])*256)
00115 
00116         # convert 16 bit samples to signed 16 bits samples
00117         for i in range (0,len(mictmp)) :
00118             if mictmp[i]>=32768 :
00119                 mictmp[i]=mictmp[i]-65536
00120 
00121         if self.config['use_ros_time']:
00122             audio_msg.header.stamp = rospy.Time.now()
00123         else:
00124             audio_msg.header.stamp = rospy.Time(timeStamp)
00125 
00126         audio_msg.frequency = self.config['frequency']
00127         if self.microVersion == 0:
00128             channels = [0,2,1,4]
00129         else:
00130             channels = [3,5,0,2]
00131 
00132         audio_msg.channelMap = channels
00133 
00134         audio_msg.data = mictmp
00135         self.pub_audio_.publish(audio_msg)


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 12:51:48