Merged Audio branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@560 f169173b-cf89-36c8-b27e-44dbe73f0c83

rtabmap_audio/AudioPlayerNode.cpp

@@ -0,0 +1,389 @@
+/*
+ * AudioPlayerNode.cpp
+ */
+
+#include <fmod.hpp>
+#include <fmod_errors.h>
+#include <ros/ros.h>
+#include <utilite/ULogger.h>
+#include <utilite/UMath.h>
+#include <utilite/UThreadNode.h>
+#include <utilite/UPlot.h>
+#include <utilite/USpectrogram.h>
+#include "rtabmap_audio/AudioFrame.h"
+#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
+
+#include <QApplication>
+#include <signal.h>
+
+#define DEFAULT_GUI_USED true
+#define PLOT_SAMPLING_RATIO 64
+
+// Ring buffer
+unsigned int g_readPtr = 0;
+unsigned int g_writePtr = 0;
+std::vector<char> g_ringBuffer; // interleaved audio
+unsigned int g_readingLooped = 0;
+unsigned int g_writingLooped = 0;
+bool g_warn = true;
+
+// Fmod stuff
+FMOD::System           *g_system = 0;
+FMOD::Sound            *g_sound = 0;
+FMOD::Channel          *g_channel = 0;
+bool                    initialized = false;
+
+// Display stuff
+UPlot * g_plot = 0;
+UPlotCurve * g_curveReceiving = 0;
+UPlotCurve * g_curvePlaying = 0;
+USpectrogram * g_spectrogram = 0;
+
+void ERRCHECK(FMOD_RESULT result)
+{
+    if (result != FMOD_OK)
+    {
+    	ROS_ERROR("FMOD error! (%d) %s\n", result, FMOD_ErrorString(result));
+        exit(-1);
+    }
+}
+
+void updateCurve(UPlotCurve * curve, void * data, unsigned int dataSize, int channels, int sampleSize)
+{
+	if(!curve || !data || dataSize == 0)
+	{
+		return;
+	}
+	//ROS_INFO("channels=%d, bitsPerSample=%d", channels, sampleSize);
+	int downSamplingFactor = PLOT_SAMPLING_RATIO*channels;
+	QVector<int> v(dataSize/downSamplingFactor/sampleSize);
+	//ROS_INFO("dataSize=%d downSamplingFactor = %d, v=%d", dataSize, downSamplingFactor, v.size());
+	if(sampleSize == 1)
+	{
+		char * p = (char*)data;
+		char min,max;
+		for(int i=0; i<v.size(); ++i)
+		{
+			uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
+			v[i] = abs(min) > abs(max) ? min : max;
+		}
+	}
+	else if(sampleSize == 2)
+	{
+		short * p = (short*)data;
+		short min,max;
+		for(int i=0; i<v.size(); ++i)
+		{
+			uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
+			v[i] = abs(min) > abs(max) ? min : max;
+		}
+	}
+	else if(sampleSize == 4)
+	{
+		int * p = (int*)data;
+		int min,max;
+		for(int i=0; i<v.size(); ++i)
+		{
+			uMinMax(p + i*downSamplingFactor, downSamplingFactor, min, max);
+			v[i] = abs(min) > abs(max) ? min : max;
+		}
+	}
+	QMetaObject::invokeMethod(curve, "addValues", Q_ARG(QVector<int>, v));
+}
+
+FMOD_RESULT F_CALLBACK pcmreadcallback(FMOD_SOUND *sound, void *data, unsigned int datalen)
+{
+	//ROS_INFO("datalen=%d, g_readPtr=%d", datalen, g_readPtr);
+    unsigned int  count;
+    char * buffer = (char *)data;
+
+    bool okToCpy = false;
+
+    if(g_readPtr < g_writePtr)
+    {
+    	okToCpy = true;
+    }
+    else if(g_readingLooped < g_writingLooped)
+    {
+    	okToCpy = true;
+    }
+
+    if(okToCpy)
+    {
+    	g_warn = true;
+    	unsigned int start = g_readPtr;
+		for(count=0; count<datalen; ++count)
+		{
+			*(buffer++) = g_ringBuffer[g_readPtr++];
+			g_readPtr %= g_ringBuffer.size();
+		}
+		if(g_readPtr < start)
+		{
+			++g_readingLooped;
+		}
+
+		if(g_plot && g_plot->isVisible() && g_curvePlaying)
+		{
+			int channels, bitsPerSample;
+			FMOD_Sound_GetFormat(sound, 0, 0, &channels, &bitsPerSample);
+			updateCurve(g_curvePlaying, &g_ringBuffer[start], datalen, channels, bitsPerSample/8);
+		}
+		//ROS_INFO("datalen=%d, writePtr=%d, readPtr=%d", datalen, g_writePtr, g_readPtr);
+    }
+    else if(g_warn)
+    {
+    	g_warn = false;
+    	ROS_WARN("Empty buffer : stream down? (this warning is shown only one time)");
+
+    	//fill data with zeros
+    	for(unsigned int i=0; i<datalen; ++i)
+		{
+			*(buffer++) = 0;
+		}
+    }
+
+    return FMOD_OK;
+}
+
+
+FMOD_RESULT F_CALLBACK pcmsetposcallback(FMOD_SOUND *sound, int subsound, unsigned int position, FMOD_TIMEUNIT postype)
+{
+    /*
+        This is useful if the user calls Sound::setPosition and you want to seek your data accordingly.
+    */
+    return FMOD_OK;
+}
+
+/**
+ * decoderBufferSize = in bytes
+ */
+bool initAudioPlayer(unsigned int decoderBufferSize, int fs, int channels, int bytesPerSample)
+{
+	ROS_INFO("Init player with fs=%d, channels=%d, bytesPerSample=%d", fs, channels, bytesPerSample);
+	UASSERT(bytesPerSample == 1 || bytesPerSample == 2 || bytesPerSample == 4);
+	FMOD_RESULT result;
+	FMOD_CREATESOUNDEXINFO createsoundexinfo;
+	unsigned int version;
+	FMOD_MODE mode = FMOD_2D | FMOD_OPENUSER | FMOD_LOOP_NORMAL | FMOD_HARDWARE | FMOD_CREATESTREAM;
+	/*
+			Create a System object and initialize.
+	*/
+	result = FMOD::System_Create(&g_system);
+	ERRCHECK(result);
+
+	result = g_system->getVersion(&version);
+	ERRCHECK(result);
+
+	if (version < FMOD_VERSION)
+	{
+		printf("Error!  You are using an old version of FMOD %08x.  This program requires %08x\n", version, FMOD_VERSION);
+		return false;
+	}
+
+	result = g_system->init(32, FMOD_INIT_NORMAL, 0);
+	ERRCHECK(result);
+
+	 memset(&createsoundexinfo, 0, sizeof(FMOD_CREATESOUNDEXINFO));
+	createsoundexinfo.cbsize            = sizeof(FMOD_CREATESOUNDEXINFO);              /* required. */
+	createsoundexinfo.length            = decoderBufferSize;     		   /* Length of PCM data in bytes of whole song (for Sound::getLength) */
+	createsoundexinfo.numchannels       = channels;                                    /* Number of channels in the sound. */
+	createsoundexinfo.defaultfrequency  = fs;                                       /* Default playback rate of sound. */
+	createsoundexinfo.decodebuffersize  = (decoderBufferSize / bytesPerSample) / channels;                  /* Chunk size of stream update in samples.  This will be the amount of data passed to the user callback. */
+	if(bytesPerSample == 1)
+	{
+		createsoundexinfo.format            = FMOD_SOUND_FORMAT_PCM8;               /* Data format of sound. */
+	}
+	else if(bytesPerSample == 2)
+	{
+		createsoundexinfo.format            = FMOD_SOUND_FORMAT_PCM16;               /* Data format of sound. */
+	}
+	else if(bytesPerSample == 4)
+	{
+		createsoundexinfo.format            = FMOD_SOUND_FORMAT_PCM32;                 /* Data format of sound. */
+	}
+	else
+	{
+		ROS_ERROR("Sample size format (%d) must be 1, 2 or 4", bytesPerSample);
+		return false;
+	}
+	createsoundexinfo.pcmreadcallback   = pcmreadcallback;                             /* User callback for reading. */
+	createsoundexinfo.pcmsetposcallback = pcmsetposcallback;                           /* User callback for seeking. */
+
+	result = g_system->createSound(0, mode, &createsoundexinfo, &g_sound);
+	ERRCHECK(result);
+
+	/*
+		Play the sound.
+	*/
+
+	result = g_system->playSound(FMOD_CHANNEL_FREE, g_sound, 0, &g_channel);
+	ERRCHECK(result);
+
+	return true;
+}
+
+void frameReceivedCallback(const rtabmap_audio::AudioFramePtr & msg)
+{
+	if(msg->data.size())
+	{
+		if(!g_ringBuffer.size())
+		{
+			g_ringBuffer = std::vector<char>(msg->data.size() * 5 * msg->nChannels * msg->sampleSize, 0);
+		}
+
+		//ROS_INFO("Received audio frame size=(%d), writePtr=%d", msg->data.size(), g_writePtr);
+		unsigned int start = g_writePtr;
+		if(g_writePtr + msg->data.size() <= g_ringBuffer.size())
+		{
+			memcpy(g_ringBuffer.data()+g_writePtr, msg->data.data(), msg->data.size());
+			g_writePtr += msg->data.size();
+			g_writePtr %= g_ringBuffer.size();
+		}
+		else
+		{
+			unsigned int size2 = (g_writePtr + msg->data.size()) - g_ringBuffer.size();
+			unsigned int size1 = msg->data.size() - size2;
+			memcpy(g_ringBuffer.data()+g_writePtr, msg->data.data(), size1);
+			memcpy(g_ringBuffer.data(), msg->data.data() + size1, size2);
+			g_writePtr = size2;
+		}
+		if(g_writePtr < start)
+		{
+			++g_writingLooped;
+		}
+
+		if(!initialized)
+		{
+			//wait for second frame
+			if(g_writePtr > msg->data.size() * 2)
+			{
+				initialized = initAudioPlayer(msg->data.size(), msg->fs, msg->nChannels, msg->sampleSize);
+				if(!initialized)
+				{
+					ROS_ERROR("Cannot initialize the audio player...");
+					exit(-1);
+				}
+			}
+			else if(g_plot)
+			{
+				if(g_plot->isVisible() && ((msg->data.size() / msg->nChannels) / msg->sampleSize) % PLOT_SAMPLING_RATIO != 0)
+				{
+					g_plot->setVisible(false);
+					ROS_WARN("Frame length (%d) must be a multiple of %d to show audio plot...", ((msg->data.size() / msg->nChannels) / msg->sampleSize), PLOT_SAMPLING_RATIO);
+				}
+				else if(g_curveReceiving && g_curvePlaying)
+				{
+					QMetaObject::invokeMethod(g_curveReceiving, "setXIncrement", Q_ARG(float, float(PLOT_SAMPLING_RATIO)/float(msg->fs)));
+					QMetaObject::invokeMethod(g_curvePlaying, "setXIncrement", Q_ARG(float, float(PLOT_SAMPLING_RATIO)/float(msg->fs)));
+				}
+			}
+		}
+		if(g_plot && g_plot->isVisible() && g_curveReceiving)
+		{
+			updateCurve(g_curveReceiving, msg->data.data(), msg->data.size(), msg->nChannels, msg->sampleSize);
+		}
+	}
+}
+
+void frameFreqSqrdMagnReceivedCallback(const rtabmap_audio::AudioFrameFreqSqrdMagnPtr & msg)
+{
+	if(msg->data.size() && msg->nChannels && g_spectrogram && g_spectrogram->isVisible())
+	{
+		QMetaObject::invokeMethod(g_spectrogram, "setSamplingRate", Q_ARG(int, msg->fs));
+		std::vector<float> data(msg->data.size()/msg->nChannels);
+		memcpy(data.data(), msg->data.data(), data.size()*sizeof(float)); // Just copy the first channel TODO support more channels...
+		QMetaObject::invokeMethod(g_spectrogram, "push", Q_ARG(std::vector<float>, data));
+	}
+}
+
+void my_handler(int s){
+	QApplication::closeAllWindows();
+}
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kDebug);
+	ros::init(argc, argv, "audioPlayer");
+
+	ros::NodeHandle nh("~");
+
+	//Parameter
+	bool guiUsed = DEFAULT_GUI_USED;
+	nh.param("gui_used", guiUsed, guiUsed);
+	ROS_INFO("gui_used=%s", guiUsed?"true":"false");
+
+	nh = ros::NodeHandle("");
+	ros::Subscriber audioSubs = nh.subscribe("audioFrame", 1, frameReceivedCallback);
+	ros::Subscriber audioFreqSqrdMagnSubs;
+
+	if(guiUsed)
+	{
+		QApplication app(argc, argv);
+
+		qRegisterMetaType<QVector<int> >("QVector<int>");
+		qRegisterMetaType<std::vector<float> >("std::vector<float>");
+
+		g_plot = new UPlot();
+		g_plot->keepAllData(false);
+		g_plot->setMaxVisibleItems(1024);
+		g_plot->setXLabel("Time (s)");
+		g_curveReceiving = g_plot->addCurve("Receiving"); // debugging purpose, to see what is received
+		g_curvePlaying = g_plot->addCurve("Playing");
+		g_plot->setMinimumSize(600, 400);
+
+		g_spectrogram = new USpectrogram();
+		g_spectrogram->setMinimumSize(640, 480);
+
+		g_spectrogram->show();
+		g_plot->show();
+
+		audioFreqSqrdMagnSubs = nh.subscribe("audioFrameFreqSqrdMagn", 1, frameFreqSqrdMagnReceivedCallback);
+
+		// Catch ctrl-c to close the gui
+		// (Place this after QApplication's constructor)
+		struct sigaction sigIntHandler;
+		sigIntHandler.sa_handler = my_handler;
+		sigemptyset(&sigIntHandler.sa_mask);
+		sigIntHandler.sa_flags = 0;
+		sigaction(SIGINT, &sigIntHandler, NULL);
+
+		ros::AsyncSpinner spinner(4); // Use 4 threads
+		spinner.start();
+		app.exec();
+		spinner.stop();
+	}
+	else
+	{
+		ros::spin();
+	}
+
+	uSleep(100); // make sure all subscribers have terminated
+	/*
+		Shut down
+	*/
+	FMOD_RESULT result;
+	if(g_sound)
+	{
+		result = g_sound->release();
+		ERRCHECK(result);
+	}
+	if(g_system)
+	{
+		result = g_system->close();
+		ERRCHECK(result);
+		result = g_system->release();
+		ERRCHECK(result);
+	}
+
+	if(g_plot)
+	{
+		delete g_plot;
+	}
+	if(g_spectrogram)
+	{
+		delete g_spectrogram;
+	}
+
+	return 0;
+}

rtabmap_audio/AudioRecorderNode.cpp

@@ -0,0 +1,272 @@
+/*
+ * AudioRecorderNode.cpp
+ */
+
+#include <ros/ros.h>
+
+#include <utilite/ULogger.h>
+#include <utilite/UFile.h>
+#include <utilite/UEventsHandler.h>
+#include <utilite/UEventsManager.h>
+#include <utilite/UEvent.h>
+#include <utilite/UThreadNode.h>
+
+#include <std_msgs/Empty.h>
+
+#include <rtabmap/core/Micro.h>
+
+#include "rtabmap_audio/AudioFrame.h"
+#include "rtabmap_audio/AudioFrameFreq.h"
+#include "rtabmap_audio/AudioFrameFreqSqrdMagn.h"
+
+#define DEFAULT_DEVICE_ID 0
+#define DEFAULT_FILE_NAME ""
+#define DEFAULT_FRAME_LENGTH 4800
+#define DEFAULT_FS 48000
+#define DEFAULT_SAMPLE_SIZE 2
+#define DEFAULT_CHANNELS 1
+
+class EndEvent : public UEvent
+{
+public:
+	virtual std::string getClassName() const {return "EndEvent";}
+};
+
+class MicroWrapper : public UThreadNode
+{
+public:
+	MicroWrapper(int deviceId, int frameLength, int fs, int sampleSize, int nChannels)
+	{
+		micro_ = new rtabmap::Micro(rtabmap::MicroEvent::kTypeFrame, deviceId, fs, frameLength, nChannels, sampleSize, 0);
+		ros::NodeHandle nh("");
+		audioFramePublisher_ = nh.advertise<rtabmap_audio::AudioFrame>("audioFrame", 1);
+		audioFrameFreqPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreq>("audioFrameFreq", 1);
+		audioFrameFreqSqrdMagnPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreqSqrdMagn>("audioFrameFreqSqrdMagn", 1);
+	}
+
+	MicroWrapper(const std::string & fileName, int frameLength)
+	{
+		micro_ = new rtabmap::Micro(rtabmap::MicroEvent::kTypeFrame, fileName, true, frameLength, 0);
+		ros::NodeHandle nh("");
+		audioFramePublisher_ = nh.advertise<rtabmap_audio::AudioFrame>("audioFrame", 1);
+		audioFrameFreqPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreq>("audioFrameFreq", 1);
+		audioFrameFreqSqrdMagnPublisher_ = nh.advertise<rtabmap_audio::AudioFrameFreqSqrdMagn>("audioFrameFreqSqrdMagn", 1);
+	}
+
+	virtual ~MicroWrapper()
+	{
+		this->join(true);
+		micro_->join(true);
+		delete micro_;
+	}
+
+	bool init()
+	{
+		if(micro_)
+		{
+			return micro_->init();
+		}
+		return false;
+	}
+
+	unsigned int freq() const
+	{
+		if(micro_)
+		{
+			return micro_->fs();
+		}
+		return 0;
+	}
+
+	unsigned int sampleSize() const
+	{
+		return micro_->bytesPerSample();
+	}
+
+	unsigned int nChannels() const
+	{
+		return micro_->channels();
+	}
+
+protected:
+	virtual void mainLoopBegin()
+	{
+		micro_->startRecorder();
+	}
+
+	virtual void mainLoop()
+	{
+		if(!micro_)
+		{
+			UERROR("micro_ is not initialized");
+			this->kill();
+		}
+		bool computeFFT = false;
+		if(audioFrameFreqPublisher_.getNumSubscribers() || audioFrameFreqSqrdMagnPublisher_.getNumSubscribers())
+		{
+			computeFFT = true;
+		}
+
+		cv::Mat data;
+		cv::Mat freq;
+		if(!computeFFT)
+		{
+			data = micro_->getFrame();
+		}
+		else
+		{
+			data = micro_->getFrame(freq, false);
+		}
+		if(!data.empty())
+		{
+			ros::Time now = ros::Time::now();
+			if(audioFramePublisher_.getNumSubscribers())
+			{
+				rtabmap_audio::AudioFramePtr msg(new rtabmap_audio::AudioFrame);
+				msg->header.frame_id = "micro";
+				msg->header.stamp = now;
+				msg->data.resize(data.total()*data.elemSize());
+				// Interleave the data
+				for(unsigned int i=0; i<msg->data.size(); i+=data.elemSize()*data.rows)
+				{
+					for(int j=0; j<data.rows; ++j)
+					{
+						memcpy(msg->data.data()+i+j*data.elemSize(), data.data + (i/(data.elemSize()*data.rows))*data.elemSize() + j*data.cols*data.elemSize(), data.elemSize());
+					}
+				}
+				msg->frameLength = data.cols;
+				msg->fs = micro_->fs();
+				msg->nChannels = data.rows;
+				msg->sampleSize = data.elemSize();
+				audioFramePublisher_.publish(msg);
+			}
+			if(audioFrameFreqPublisher_.getNumSubscribers())
+			{
+				rtabmap_audio::AudioFrameFreqPtr msg(new rtabmap_audio::AudioFrameFreq);
+				msg->header.frame_id = "micro";
+				msg->header.stamp = now;
+				msg->data.resize(freq.total()*freq.elemSize());
+				memcpy(msg->data.data(), freq.data, msg->data.size());
+				msg->frameLength = freq.cols;
+				msg->fs = micro_->fs();
+				msg->nChannels = freq.rows;
+				audioFrameFreqPublisher_.publish(msg);
+			}
+			if(audioFrameFreqSqrdMagnPublisher_.getNumSubscribers())
+			{
+				rtabmap_audio::AudioFrameFreqSqrdMagnPtr msg(new rtabmap_audio::AudioFrameFreqSqrdMagn);
+				msg->header.frame_id = "micro";
+				msg->header.stamp = now;
+
+				//compute the squared magnitude
+				cv::Mat sqrdMagn(freq.rows, freq.cols/2, CV_32F);
+				//for each channels
+				for(int i=0; i<sqrdMagn.rows; ++i)
+				{
+					cv::Mat rowFreq = freq.row(i);
+					cv::Mat rowSqrdMagn = sqrdMagn.row(i);
+					float re;
+					float im;
+					for(int j=0; j<rowSqrdMagn.cols; ++j)
+					{
+						re = rowFreq.at<float>(0, j*2);
+						im = rowFreq.at<float>(0, j*2+1);
+						rowSqrdMagn.at<float>(0, j) = re*re + im*im;
+					}
+				}
+
+				msg->data.resize(sqrdMagn.cols * sqrdMagn.rows);
+				memcpy(msg->data.data(), sqrdMagn.data, sqrdMagn.total() * sqrdMagn.elemSize());
+				msg->frameLength = sqrdMagn.cols;
+				msg->fs = micro_->fs();
+				msg->nChannels = sqrdMagn.rows;
+
+				audioFrameFreqSqrdMagnPublisher_.publish(msg);
+			}
+		}
+		else
+		{
+			UEventsManager::post(new EndEvent());
+			this->kill();
+		}
+	}
+
+private:
+	ros::Publisher audioFramePublisher_;
+	ros::Publisher audioFrameFreqPublisher_;
+	ros::Publisher audioFrameFreqSqrdMagnPublisher_;
+	rtabmap::Micro * micro_;
+};
+
+class Handler: public UEventsHandler
+{
+public:
+	Handler() {UEventsManager::addHandler(this);}
+	virtual ~Handler() {UEventsManager::removeHandler(this);}
+protected:
+	virtual void handleEvent(UEvent * event)
+	{
+		if(event->getClassName().compare("EndEvent") == 0)
+		{
+			ROS_INFO("End of stream reached... shutting down!");
+			ros::shutdown();
+		}
+	}
+};
+
+int main(int argc, char** argv)
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	//ULogger::setLevel(ULogger::kDebug);
+	ros::init(argc, argv, "audioRecorder");
+	ros::NodeHandle nh("~");
+
+	int deviceId = DEFAULT_DEVICE_ID;
+	std::string fileName = DEFAULT_FILE_NAME;
+	int frameLength = DEFAULT_FRAME_LENGTH;
+	int fs = DEFAULT_FS;
+	int sampleSize = DEFAULT_SAMPLE_SIZE;
+	int nChannels = DEFAULT_CHANNELS;
+
+	nh.param("device_id", deviceId, deviceId);
+	nh.param("file_name", fileName, fileName);
+	nh.param("frame_length", frameLength, frameLength);
+	nh.param("fs", fs, fs);
+	nh.param("sample_size", sampleSize, sampleSize);
+	nh.param("channels", nChannels, nChannels);
+
+	MicroWrapper * micro;
+	if(fileName.size() && UFile::exists(fileName))
+	{
+		ROS_INFO("Recording from file %s", fileName.c_str());
+		micro = new MicroWrapper(fileName, frameLength);
+	}
+	else
+	{
+		ROS_INFO("Recording from microphone %d", deviceId);
+		micro = new MicroWrapper(deviceId, frameLength, fs, sampleSize, nChannels);
+	}
+
+	if(!micro->init())
+	{
+		ROS_ERROR("Cannot initiate the audio recorder.");
+	}
+	else
+	{
+		ROS_INFO("frame_length=%d", frameLength);
+		ROS_INFO("fs=%d", micro->freq());
+		ROS_INFO("sample_size=%d", micro->sampleSize());
+		ROS_INFO("channels=%d", micro->nChannels());
+
+		Handler h;
+		// Start the mic
+		micro->start();
+		ROS_INFO("Audio recorder started...");
+		ros::spin();
+	}
+
+	micro->join(true);
+	delete micro;
+
+	return 0;
+}

rtabmap_audio/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+
+project(rtabmap-audio-pkg)
+
+# Set the build type.  Options are:
+#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
+#  Debug          : w/ debug symbols, w/o optimization
+#  Release        : w/o debug symbols, w/ optimization
+#  RelWithDebInfo : w/ debug symbols, w/ optimization
+#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
+#set(ROS_BUILD_TYPE RelWithDebInfo)
+
+rosbuild_init()
+
+#set the default path for built executables to the "bin" directory
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+#set the default path for built libraries to the "lib" directory
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}")
+
+#uncomment if you have defined messages
+rosbuild_genmsg()
+#uncomment if you have defined services
+#rosbuild_gensrv()
+
+#common commands for building c++ executables and libraries
+#rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
+#target_link_libraries(${PROJECT_NAME} another_library)
+#rosbuild_add_boost_directories()
+#rosbuild_link_boost(${PROJECT_NAME} thread)
+#rosbuild_add_executable(example examples/example.cpp)
+#target_link_libraries(example ${PROJECT_NAME})
+
+rosbuild_add_executable(audio_recorder AudioRecorderNode.cpp)
+target_link_libraries(audio_recorder)
+
+FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
+find_package(Fmodex REQUIRED)
+INCLUDE(${QT_USE_FILE})
+
+rosbuild_add_executable(audio_player AudioPlayerNode.cpp ${Fmodex_INCLUDE_DIRS})
+target_link_libraries(audio_player ${Fmodex_LIBRARIES} ${QT_LIBRARIES})

rtabmap_audio/FindFmodex.cmake

@@ -0,0 +1,33 @@
+# - Find Fmodex
+# This module finds an installed Fmod package.
+#
+# It sets the following variables:
+#  Fmodex_FOUND       - Set to false, or undefined, if Fmod isn't found.
+#  Fmodex_INCLUDE_DIRS - The Fmod include directory.
+#  Fmodex_LIBRARIES    - The Fmod library to link against.
+#
+# 
+     
+FIND_PATH(Fmodex_INCLUDE_DIRS fmod.h)
+
+IF(CMAKE_SIZEOF_VOID_P EQUAL 8)
+   FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex64)
+ELSE(CMAKE_SIZEOF_VOID_P EQUAL 8)
+   FIND_LIBRARY(Fmodex_LIBRARIES NAMES fmodex)
+ENDIF(CMAKE_SIZEOF_VOID_P EQUAL 8)
+
+IF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES)
+   SET(Fmodex_FOUND TRUE)
+ENDIF (Fmodex_INCLUDE_DIRS AND Fmodex_LIBRARIES)
+
+IF (Fmodex_FOUND)
+   # show which Fmod was found only if not quiet
+   IF (NOT Fmodex_FIND_QUIETLY)
+      MESSAGE(STATUS "Found Fmod: ${Fmodex_LIBRARIES}")
+   ENDIF (NOT Fmodex_FIND_QUIETLY)
+ELSE (Fmodex_FOUND)
+   # fatal error if Fmod is required but not found
+   IF (Fmodex_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find Fmod...")
+   ENDIF (Fmodex_FIND_REQUIRED)
+ENDIF (Fmodex_FOUND)

rtabmap_audio/Makefile

@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk

rtabmap_audio/mainpage.dox

@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b audio 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

rtabmap_audio/manifest.xml

@@ -0,0 +1,20 @@
+<package>
+  <description brief="rtabmap_audio">
+
+     audio
+
+  </description>
+  <author>Mathieu Labbé</author>
+  <license>BSD</license>
+  <review status="unreviewed" notes=""/>
+  <url>http://ros.org/wiki/rtabmap_audio</url>
+
+  <depend package="std_msgs"/>
+  <depend package="roscpp"/>
+  <depend package="rtabmap_lib"/>
+
+  <rosdep name="libqt4-dev"/>
+
+</package>
+
+

rtabmap_audio/msg/AudioFrame.msg

@@ -0,0 +1,11 @@
+########################################
+# Audio frame in time domain (raw)
+########################################
+
+Header header
+
+uint32 sampleSize # bytes per sample
+uint32 frameLength
+uint32 nChannels
+uint32 fs
+uint8[] data

rtabmap_audio/msg/AudioFrameFreq.msg

@@ -0,0 +1,10 @@
+########################################
+# Audio frame in frequency domain (with real and imaginary parts)
+########################################
+
+Header header
+
+uint32 frameLength
+uint32 nChannels
+uint32 fs
+float32[] data

rtabmap_audio/msg/AudioFrameFreqSqrdMagn.msg

@@ -0,0 +1,10 @@
+########################################
+# Audio frame in frequency domain (squared magnitude)
+########################################
+
+Header header
+
+uint32 frameLength
+uint32 nChannels
+uint32 fs
+float32[] data