[DEV] add a basic example on how to publish messga eon a stream and receive it ... ==> mut be doen proper now ...

audio_core/CMakeLists.txt

@@ -44,7 +44,6 @@ add_service_files(
   FILES
   create.srv
   remove.srv
-  write.srv
   getBufferTime.srv
 )
 

audio_core/src/main.cpp

@@ -105,6 +105,198 @@ class InterfaceInput {
 		}
 };
 
+class InterfaceOutputStreamElement {
+	private:
+		int32_t m_id;
+	public:
+		int32_t getId() {
+			return m_id;
+		}
+	private:
+		int32_t m_nbConsecutiveUnderflow;
+	public:
+		int32_t getCountUnderflow() {
+			return m_nbConsecutiveUnderflow;
+		}
+	private:
+		std11::shared_ptr<river::Manager> m_manager;
+		std11::shared_ptr<river::Interface> m_interface;
+		std11::mutex m_mutex;
+	public:
+		InterfaceOutputStreamElement(const std11::shared_ptr<river::Manager>& _manager, int32_t _id) :
+		  m_id(_id),
+		  m_nbConsecutiveUnderflow(0),
+		  m_manager(_manager) {
+			APPL_INFO("Create interface");
+		}
+		~InterfaceOutputStreamElement() {
+			std11::unique_lock<std::mutex> lock(m_mutex);
+			APPL_INFO("Remove interfaces (start)");
+			m_interface->stop();
+			m_interface.reset();
+			m_manager.reset();
+			APPL_INFO("Remove interfaces (done)");
+		}
+		void onTopicMessage(const std::string& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg) {
+			std11::unique_lock<std::mutex> lock(m_mutex);
+			if (m_interface != nullptr) {
+				APPL_VERBOSE("Write data : " << m_id << " size= " << _msg->data.size()/m_interface->getInterfaceFormat().getChunkSize());
+				m_interface->write(&_msg->data[0], _msg->data.size()/m_interface->getInterfaceFormat().getChunkSize());
+				m_nbConsecutiveUnderflow = 0;
+				return;
+			}
+			audio::format format = audio::convertFormat(_msg->channelFormat);
+			std::vector<enum audio::channel> map = audio::convertChannel(_msg->channelMap);
+			// no interface found => create a new one
+			m_interface = m_manager->createOutput(_msg->frequency,
+			                                      map,
+			                                      format,
+			                                      _streamName);
+			if(m_interface == nullptr) {
+				APPL_ERROR("nullptr interface");
+				return;
+			}
+			m_interface->setReadwrite();
+			m_interface->setStatusFunction(std11::bind(&InterfaceOutputStreamElement::onStatus, this, std11::placeholders::_1, std11::placeholders::_2, _msg->sourceId));
+			m_interface->start();
+			m_interface->write(&_msg->data[0], _msg->data.size()/m_interface->getInterfaceFormat().getChunkSize());
+		}
+		void onStatus(const std::string& _origin, const std::string& _status, int32_t _iii) {
+			APPL_VERBOSE("status event : " << _origin << " status=" << _status << " on i=" << _iii);
+			m_nbConsecutiveUnderflow++;
+		}
+};
+
+
+class InterfaceOutputStreamManager {
+	private:
+		std::string m_name;
+	public:
+		const std::string& getName() {
+			return m_name;
+		}
+	private:
+		std11::shared_ptr<river::Manager> m_manager;
+		std::vector<std11::shared_ptr<InterfaceOutputStreamElement> > m_elementList;
+		std11::mutex m_mutex;
+	public:
+		InterfaceOutputStreamManager(const std::string& _name) :
+		  m_name(_name) {
+			m_manager = river::Manager::create(m_name);
+			APPL_INFO("Create Manager : " << m_name);
+		}
+		~InterfaceOutputStreamManager() {
+			APPL_INFO("Remove Manager : " << m_name);
+			std11::unique_lock<std::mutex> lock(m_mutex);
+			APPL_INFO("Clean list");
+			m_elementList.clear();
+			m_manager.reset();
+			APPL_INFO("All is done ...");
+		}
+		void onTopicMessage(const std::string& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg) {
+			std11::unique_lock<std::mutex> lock(m_mutex);
+			for (size_t iii=0; iii<m_elementList.size(); ++iii) {
+				if (m_elementList[iii] == nullptr) {
+					continue;
+				}
+				if(m_elementList[iii]->getId() == _msg->sourceId) {
+					m_elementList[iii]->onTopicMessage(_streamName, _msg);
+					return;
+				}
+			}
+			// no interface found => create a new one
+			std11::shared_ptr<InterfaceOutputStreamElement> interface = std11::make_shared<InterfaceOutputStreamElement>(m_manager, _msg->sourceId);
+			if(interface == nullptr) {
+				APPL_ERROR("nullptr interface");
+				return;
+			}
+			m_elementList.push_back(interface);
+			interface->onTopicMessage(_streamName, _msg);
+			m_manager->generateDotAll("myDot.dot");
+		}
+		bool onTimer() {
+			std::vector<std11::shared_ptr<InterfaceOutputStreamElement> >::iterator it = m_elementList.begin();
+			bool oneElementRemoved = false;
+			while (it != m_elementList.end()) {
+				if (*it == nullptr) {
+					it = m_elementList.erase(it);
+					continue;
+				}
+				if ((*it)->getCountUnderflow() >= 50) {
+					(*it).reset();
+					oneElementRemoved = true;
+					it = m_elementList.erase(it);
+					continue;
+				}
+				++it;
+			}
+			if (oneElementRemoved == true) {
+				m_manager->generateDotAll("myDot.dot");
+			}
+			// return remove ...
+			return m_elementList.size() == 0;
+		}
+};
+
+class InterfaceOutputStream {
+	public:
+		std::string m_lowLevelStreamName;
+		ros::Subscriber m_stream;
+		ros::Timer m_timer;
+		std11::mutex m_mutex;
+		std::vector<std11::shared_ptr<InterfaceOutputStreamManager> > m_list;
+	public:
+		InterfaceOutputStream(const std::string& _input="speaker", const std::string& _publisher="speaker") :
+		  m_lowLevelStreamName("speaker") {
+			ros::NodeHandle nodeHandlePrivate("~");
+			m_stream = nodeHandlePrivate.subscribe<audio_msg::AudioBuffer>(_publisher,
+			                                                               1000,
+			                                                               boost::bind(&InterfaceOutputStream::onTopicMessage, this, _1));
+			m_timer = nodeHandlePrivate.createTimer(ros::Duration(ros::Rate(4)), boost::bind(&InterfaceOutputStream::onTimer, this, _1));
+		}
+		~InterfaceOutputStream() {
+			std11::unique_lock<std::mutex> lock(m_mutex);
+			m_list.clear();
+		}
+		void onTopicMessage(const audio_msg::AudioBuffer::ConstPtr& _msg) {
+			std11::unique_lock<std::mutex> lock(m_mutex);
+			for (size_t iii=0; iii<m_list.size(); ++iii) {
+				if (m_list[iii] == nullptr) {
+					continue;
+				}
+				if (m_list[iii]->getName() == _msg->sourceName) {
+					APPL_VERBOSE("Write data : " << _msg->sourceName);
+					m_list[iii]->onTopicMessage(m_lowLevelStreamName, _msg);
+					return;
+				}
+			}
+			// new interface name:
+			std11::shared_ptr<InterfaceOutputStreamManager> newInterface = std11::make_shared<InterfaceOutputStreamManager>(_msg->sourceName);
+			if (newInterface == nullptr) {
+				APPL_ERROR("can not generate new interface element...");
+				return;
+			}
+			m_list.push_back(newInterface);
+			newInterface->onTopicMessage(m_lowLevelStreamName, _msg);
+		}
+		void onTimer(const ros::TimerEvent& _timer) {
+			std11::unique_lock<std::mutex> lock(m_mutex);
+			std::vector<std11::shared_ptr<InterfaceOutputStreamManager> >::iterator it = m_list.begin();
+			while (it != m_list.end()) {
+				if (*it == nullptr) {
+					it = m_list.erase(it);
+					continue;
+				}
+				if ((*it)->onTimer() == true) {
+					(*it).reset();
+					it = m_list.erase(it);
+					continue;
+				}
+				++it;
+			}
+		}
+};
+
 
 class InterfaceOutput {
 	public:
@@ -214,34 +406,6 @@ bool f_remove(audio_core::remove::Request& _req,
 	return true;
 }
 
-
-bool f_write(audio_core::write::Request& _req,
-             audio_core::write::Response& _res) {
-	std11::shared_ptr<InterfaceOutput> interface;
-	// reset ouput
-	_res.waitTime = 0;
-	mutex.lock();
-	// Find the handle:
-	for(size_t iii=0; iii<g_listInterafceOut.size(); ++iii) {
-		if (g_listInterafceOut[iii] == nullptr) {
-			continue;
-		}
-		if (g_listInterafceOut[iii]->getId() == _req.handle) {
-			interface = g_listInterafceOut[iii];
-			break;
-		}
-	}
-	mutex.unlock();
-	if (interface == nullptr) {
-		APPL_ERROR("write : [" << _req.handle << "] Can not write ==> handle does not exist...");
-		return false;
-	}
-	APPL_INFO("write : [" << _req.handle << "] (start)");
-	_res.waitTime = interface->write(_req.data);
-	APPL_INFO("write : [" << _req.handle << "] (end)");
-	return true;
-}
-
 bool f_getBufferTime(audio_core::getBufferTime::Request& _req,
                      audio_core::getBufferTime::Response& _res) {
 	std11::shared_ptr<InterfaceOutput> interface;
@@ -294,16 +458,15 @@ int main(int _argc, char **_argv) {
 	
 	ros::ServiceServer serviceCreate = n.advertiseService("create", f_create);
 	ros::ServiceServer serviceRemove = n.advertiseService("remove", f_remove);
-	ros::ServiceServer serviceWrite = n.advertiseService("write", f_write);
 	ros::ServiceServer serviceGetBufferTime = n.advertiseService("getBufferTime", f_getBufferTime);
 	
-	ros::NodeHandle nodeHandlePrivate("~");
+	g_manager = river::Manager::create(n.getNamespace());
-	
-	g_manager = river::Manager::create("ROS node");
 	// start publishing of Microphone
 	std11::shared_ptr<InterfaceInput> m_input = std11::make_shared<InterfaceInput>(g_manager, "microphone", "microphone", false);
 	// start publishing of Speaker feedback
-	std11::shared_ptr<InterfaceInput> m_feedback = std11::make_shared<InterfaceInput>(g_manager, "speaker", "speaker", true);
+	std11::shared_ptr<InterfaceInput> m_feedback = std11::make_shared<InterfaceInput>(g_manager, "speaker", "feedback/speaker", true);
+	// create the Stream for output
+	std11::shared_ptr<InterfaceOutputStream> m_speaker = std11::make_shared<InterfaceOutputStream>("speaker", "speaker");
 	
 	/*
 	 * A count of how many messages we have sent. This is used to create

audio_core/srv/write.srv

@@ -1,6 +0,0 @@
-# audio interface UID
-int64 handle
-# interlaced data of the audio buffer
-int8[] data
----
-uint32 waitTime

audio_msg/msg/AudioBuffer.msg

@@ -36,7 +36,7 @@ Header header
 # source
 string sourceName
 # source id
-uint32 sourceId
+int32 sourceId
 # current frequency of the audio interface
 uint16 frequency
 # channel order of the current buffer

test/test_audio_generator/src/main.cpp

@@ -7,15 +7,6 @@
 
 #include <sstream>
 
-FILE* filee = NULL;
-/**
- * This tutorial demonstrates simple receipt of messages over the ROS system.
- */
-void audioCallback(const audio_msg::AudioBuffer::ConstPtr& msg) {
-	fwrite(&msg->data[0] , sizeof(int16_t), msg->data.size(), filee);
-	ROS_INFO("get message: freq=%d nbChannel=%u nbSample=%ld", (int32_t)msg->frequency, (int32_t)msg->channelMap.size(), msg->data.size());
-}
-
 int64_t audioInterface_create(ros::NodeHandle& _n, ros::Time _timeUs, int32_t _freq, const std::vector<uint8_t> _channelMap) {
 	ros::ServiceClient client = _n.serviceClient<audio_core::create>("create");
 	audio_core::create srv;
@@ -45,24 +36,6 @@ bool audioInterface_remove(ros::NodeHandle& _n, int64_t _uid) {
 	}
 }
 
-/**
- * @brief return wait time
- */
-uint32_t audioInterface_write(ros::NodeHandle& _n, int64_t _uid, const std::vector<int16_t>& _value) {
-	ros::ServiceClient client = _n.serviceClient<audio_core::write>("write");
-	audio_core::write srv;
-	srv.request.handle = _uid;
-	srv.request.data.resize(_value.size()*2);
-	memcpy(&srv.request.data[0], &_value[0], srv.request.data.size());
-	if (client.call(srv)) {
-		ROS_INFO("write need wait time : %d", srv.response.waitTime);
-		return srv.response.waitTime;
-	} else {
-		ROS_ERROR("Failed to call service write");
-		assert(0);
-	}
-}
-
 uint32_t audioInterface_getBufferTime(ros::NodeHandle& _n, int64_t _uid) {
 	ros::ServiceClient client = _n.serviceClient<audio_core::getBufferTime>("getBufferTime");
 	audio_core::getBufferTime srv;
@@ -88,7 +61,7 @@ void usage() {
 }
 
 int main(int argc, char **argv) {
-	if (argc != 6 ) {
+	if (argc < 6 ) {
 		ROS_ERROR ("not enought argument : %d", argc);
 		usage();
 	}
@@ -136,47 +109,51 @@ int main(int argc, char **argv) {
 		ROS_ERROR("nb chnnale supported error : %d not in [1,2,3,4]", p_nbChannels);
 		exit(-1);
 	}
-	// connect:
+	// new interface: just published data:
-	int64_t uid = audioInterface_create(n, timee, p_sampleRate, channelMap);
+	ros::Publisher stream;
+	ros::NodeHandle nodeHandle;
+	// create the output stream:
+	stream = nodeHandle.advertise<audio_msg::AudioBuffer>(p_channelToPlay, 100);
+	
+	audio_msg::AudioBuffer msg;
+	// Basic source name is the curant node handle (it is unique)
+	msg.sourceName = ros::NodeHandle("~").getNamespace();
+	msg.sourceId = 0;
+	// create the Ros timestamp
+	msg.header.stamp = ros::Time::now();
+	// set message frequency
+	msg.frequency = p_sampleRate;
+	// set channel map properties
+	msg.channelMap = channelMap;
+	// Set the format of flow
+	msg.channelFormat = audio_msg::AudioBuffer::FORMAT_INT16;
+	
 	std::vector<int16_t> data;
 	data.resize(baseDataSize*channelMap.size());
-	
 	double baseCycle = 2.0*M_PI/(double)p_sampleRate * (double)p_frequency;
-	
 	int32_t generateTime = (p_timeToPlay * p_sampleRate) / baseDataSize;
-	
-	
 	for (int32_t kkk=0; kkk<generateTime; ++kkk) {
 		for (int32_t iii=0; iii<data.size()/channelMap.size(); iii++) {
 			for (int32_t jjj=0; jjj<channelMap.size(); jjj++) {
-				data[channelMap.size()*iii+jjj] = cos(phase) * 30000;
+				data[channelMap.size()*iii+jjj] = cos(phase) * 15000;
 			}
 			phase += baseCycle;
 			if (phase >= 2*M_PI) {
 				phase -= 2*M_PI;
 			}
 		}
-		//ROS_INFO("send data");
+		// copy data:
-		int32_t needSleep = audioInterface_write(n, uid, data);
+		msg.data.resize(data.size()*sizeof(int16_t));
-		if (needSleep > 0) {
+		memcpy(&msg.data[0], &data[0], data.size()*sizeof(int16_t));
-			ROS_INFO("need sleep %d", needSleep);
+		// publish message
-			usleep(needSleep);
+		stream.publish(msg);
-		} else {
+		
-			ROS_INFO("not sleep");
+		int32_t needSleep = (double(data.size()/channelMap.size()) / (double)p_sampleRate) * 1000000.0 * 0.97;
+		if (kkk >= 5) {
+			ROS_INFO_STREAM("need sleep " << needSleep << " µs for " << data.size()/channelMap.size() << " chunks");
 			usleep(needSleep);
 		}
 	}
-	// wait end if playing :
-	usleep(200000);
-	uint32_t waitTime = audioInterface_getBufferTime(n, uid);
-	while (waitTime>0) {
-		ROS_INFO("wait end of playing ... %u us", waitTime);
-		usleep(waitTime/2);
-		waitTime = audioInterface_getBufferTime(n, uid);
-	}
-	// close:
-	audioInterface_remove(n, uid);
-	
 	return 0;
 }