[DEV] update new ETK
This commit is contained in:
parent
6f0c0f2088
commit
f0e1d48838
@ -13,8 +13,8 @@
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
appl::InterfaceInput::InterfaceInput(std11::shared_ptr<audio::river::Manager> _manager,
|
||||
const std::string& _input,
|
||||
const std::string& _publisher,
|
||||
const etk::String& _input,
|
||||
const etk::String& _publisher,
|
||||
bool _feedback) :
|
||||
m_manager(_manager) {
|
||||
ros::NodeHandle nodeHandlePrivate("~");
|
||||
@ -23,9 +23,9 @@ appl::InterfaceInput::InterfaceInput(std11::shared_ptr<audio::river::Manager> _m
|
||||
boost::bind(&InterfaceInput::onConnect, this, _1),
|
||||
boost::bind(&InterfaceInput::onDisConnect, this, _1));
|
||||
//Set stereo output:
|
||||
std::vector<audio::channel> channelMap;
|
||||
channelMap.push_back(audio::channel_frontLeft);
|
||||
channelMap.push_back(audio::channel_frontRight);
|
||||
etk::Vector<audio::channel> channelMap;
|
||||
channelMap.pushBack(audio::channel_frontLeft);
|
||||
channelMap.pushBack(audio::channel_frontRight);
|
||||
if (_feedback == false) {
|
||||
m_interface = m_manager->createInput(48000,
|
||||
channelMap,
|
||||
@ -71,7 +71,7 @@ void appl::InterfaceInput::onDataReceived(const void* _data,
|
||||
size_t _nbChunk,
|
||||
enum audio::format _format,
|
||||
uint32_t _frequency,
|
||||
const std::vector<audio::channel>& _map) {
|
||||
const etk::Vector<audio::channel>& _map) {
|
||||
if (_format != audio::format_int16) {
|
||||
APPL_ERROR("call wrong type ... (need int16_t)");
|
||||
}
|
||||
|
@ -20,8 +20,8 @@ namespace appl {
|
||||
std11::mutex mutex;
|
||||
public:
|
||||
InterfaceInput(std11::shared_ptr<audio::river::Manager> _manager,
|
||||
const std::string& _input="microphone",
|
||||
const std::string& _publisher="microphone",
|
||||
const etk::String& _input="microphone",
|
||||
const etk::String& _publisher="microphone",
|
||||
bool _feedback=false);
|
||||
~InterfaceInput();
|
||||
protected:
|
||||
@ -32,7 +32,7 @@ namespace appl {
|
||||
size_t _nbChunk,
|
||||
enum audio::format _format,
|
||||
uint32_t _frequency,
|
||||
const std::vector<audio::channel>& _map);
|
||||
const etk::Vector<audio::channel>& _map);
|
||||
};
|
||||
}
|
||||
|
||||
|
@ -9,7 +9,7 @@
|
||||
#include "debug.h"
|
||||
#include "InterfaceOutput.h"
|
||||
|
||||
appl::InterfaceOutput::InterfaceOutput(const std::string& _input, const std::string& _publisher) :
|
||||
appl::InterfaceOutput::InterfaceOutput(const etk::String& _input, const etk::String& _publisher) :
|
||||
m_lowLevelStreamName(_input) {
|
||||
ros::NodeHandle nodeHandlePrivate("~");
|
||||
m_stream = nodeHandlePrivate.subscribe<audio_msg::AudioBuffer>(_publisher,
|
||||
@ -19,12 +19,12 @@ appl::InterfaceOutput::InterfaceOutput(const std::string& _input, const std::str
|
||||
}
|
||||
|
||||
appl::InterfaceOutput::~InterfaceOutput() {
|
||||
std11::unique_lock<std::mutex> lock(m_mutex);
|
||||
std11::unique_lock<ethread::Mutex> lock(m_mutex);
|
||||
m_list.clear();
|
||||
}
|
||||
|
||||
void appl::InterfaceOutput::onTopicMessage(const audio_msg::AudioBuffer::ConstPtr& _msg) {
|
||||
std11::unique_lock<std::mutex> lock(m_mutex);
|
||||
std11::unique_lock<ethread::Mutex> lock(m_mutex);
|
||||
for (size_t iii=0; iii<m_list.size(); ++iii) {
|
||||
if (m_list[iii] == nullptr) {
|
||||
continue;
|
||||
@ -41,13 +41,13 @@ void appl::InterfaceOutput::onTopicMessage(const audio_msg::AudioBuffer::ConstPt
|
||||
APPL_ERROR("can not generate new interface element...");
|
||||
return;
|
||||
}
|
||||
m_list.push_back(newInterface);
|
||||
m_list.pushBack(newInterface);
|
||||
newInterface->onTopicMessage(m_lowLevelStreamName, _msg);
|
||||
}
|
||||
|
||||
void appl::InterfaceOutput::onTimer(const ros::TimerEvent& _timer) {
|
||||
std11::unique_lock<std::mutex> lock(m_mutex);
|
||||
std::vector<std11::shared_ptr<appl::InterfaceOutputManager> >::iterator it = m_list.begin();
|
||||
std11::unique_lock<ethread::Mutex> lock(m_mutex);
|
||||
etk::Vector<std11::shared_ptr<appl::InterfaceOutputManager> >::iterator it = m_list.begin();
|
||||
while (it != m_list.end()) {
|
||||
if (*it == nullptr) {
|
||||
it = m_list.erase(it);
|
||||
|
@ -16,13 +16,13 @@
|
||||
namespace appl {
|
||||
class InterfaceOutput {
|
||||
public:
|
||||
std::string m_lowLevelStreamName;
|
||||
etk::String m_lowLevelStreamName;
|
||||
ros::Subscriber m_stream;
|
||||
ros::Timer m_timer;
|
||||
std11::mutex m_mutex;
|
||||
std::vector<std11::shared_ptr<appl::InterfaceOutputManager> > m_list;
|
||||
etk::Vector<std11::shared_ptr<appl::InterfaceOutputManager> > m_list;
|
||||
public:
|
||||
InterfaceOutput(const std::string& _input="speaker", const std::string& _publisher="speaker");
|
||||
InterfaceOutput(const etk::String& _input="speaker", const etk::String& _publisher="speaker");
|
||||
~InterfaceOutput();
|
||||
void onTopicMessage(const audio_msg::AudioBuffer::ConstPtr& _msg);
|
||||
void onTimer(const ros::TimerEvent& _timer);
|
||||
|
@ -17,7 +17,7 @@ appl::InterfaceOutputElement::InterfaceOutputElement(const std11::shared_ptr<aud
|
||||
}
|
||||
|
||||
appl::InterfaceOutputElement::~InterfaceOutputElement() {
|
||||
std11::unique_lock<std::mutex> lock(m_mutex);
|
||||
std11::unique_lock<ethread::Mutex> lock(m_mutex);
|
||||
APPL_INFO("Remove interfaces (start)");
|
||||
m_interface->stop();
|
||||
m_interface.reset();
|
||||
@ -25,8 +25,8 @@ appl::InterfaceOutputElement::~InterfaceOutputElement() {
|
||||
APPL_INFO("Remove interfaces (done)");
|
||||
}
|
||||
|
||||
void appl::InterfaceOutputElement::onTopicMessage(const std::string& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg) {
|
||||
std11::unique_lock<std::mutex> lock(m_mutex);
|
||||
void appl::InterfaceOutputElement::onTopicMessage(const etk::String& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg) {
|
||||
std11::unique_lock<ethread::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());
|
||||
@ -34,7 +34,7 @@ void appl::InterfaceOutputElement::onTopicMessage(const std::string& _streamName
|
||||
return;
|
||||
}
|
||||
audio::format format = audio::convertFormat(_msg->channelFormat);
|
||||
std::vector<enum audio::channel> map = audio::convertChannel(_msg->channelMap);
|
||||
etk::Vector<enum audio::channel> map = audio::convertChannel(_msg->channelMap);
|
||||
// no interface found => create a new one
|
||||
m_interface = m_manager->createOutput(_msg->frequency,
|
||||
map,
|
||||
@ -50,7 +50,7 @@ void appl::InterfaceOutputElement::onTopicMessage(const std::string& _streamName
|
||||
m_interface->write(&_msg->data[0], _msg->data.size()/m_interface->getInterfaceFormat().getChunkSize());
|
||||
}
|
||||
|
||||
void appl::InterfaceOutputElement::onStatus(const std::string& _origin, const std::string& _status, int32_t _iii) {
|
||||
void appl::InterfaceOutputElement::onStatus(const etk::String& _origin, const etk::String& _status, int32_t _iii) {
|
||||
APPL_VERBOSE("status event : " << _origin << " status=" << _status << " on i=" << _iii);
|
||||
m_nbConsecutiveUnderflow++;
|
||||
}
|
||||
|
@ -35,8 +35,8 @@ namespace appl {
|
||||
public:
|
||||
InterfaceOutputElement(const std11::shared_ptr<audio::river::Manager>& _manager, int32_t _id);
|
||||
~InterfaceOutputElement();
|
||||
void onTopicMessage(const std::string& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg);
|
||||
void onStatus(const std::string& _origin, const std::string& _status, int32_t _iii);
|
||||
void onTopicMessage(const etk::String& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg);
|
||||
void onStatus(const etk::String& _origin, const etk::String& _status, int32_t _iii);
|
||||
};
|
||||
}
|
||||
|
||||
|
@ -9,7 +9,7 @@
|
||||
#include "debug.h"
|
||||
#include "InterfaceOutputManager.h"
|
||||
|
||||
appl::InterfaceOutputManager::InterfaceOutputManager(const std::string& _name) :
|
||||
appl::InterfaceOutputManager::InterfaceOutputManager(const etk::String& _name) :
|
||||
m_name(_name) {
|
||||
m_manager = audio::river::Manager::create(m_name);
|
||||
APPL_INFO("Create Manager : " << m_name);
|
||||
@ -17,15 +17,15 @@ appl::InterfaceOutputManager::InterfaceOutputManager(const std::string& _name) :
|
||||
|
||||
appl::InterfaceOutputManager::~InterfaceOutputManager() {
|
||||
APPL_INFO("Remove Manager : " << m_name);
|
||||
std11::unique_lock<std::mutex> lock(m_mutex);
|
||||
std11::unique_lock<ethread::Mutex> lock(m_mutex);
|
||||
APPL_INFO("Clean list");
|
||||
m_elementList.clear();
|
||||
m_manager.reset();
|
||||
APPL_INFO("All is done ...");
|
||||
}
|
||||
|
||||
void appl::InterfaceOutputManager::onTopicMessage(const std::string& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg) {
|
||||
std11::unique_lock<std::mutex> lock(m_mutex);
|
||||
void appl::InterfaceOutputManager::onTopicMessage(const etk::String& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg) {
|
||||
std11::unique_lock<ethread::Mutex> lock(m_mutex);
|
||||
for (size_t iii=0; iii<m_elementList.size(); ++iii) {
|
||||
if (m_elementList[iii] == nullptr) {
|
||||
continue;
|
||||
@ -41,13 +41,13 @@ void appl::InterfaceOutputManager::onTopicMessage(const std::string& _streamName
|
||||
APPL_ERROR("nullptr interface");
|
||||
return;
|
||||
}
|
||||
m_elementList.push_back(interface);
|
||||
m_elementList.pushBack(interface);
|
||||
interface->onTopicMessage(_streamName, _msg);
|
||||
m_manager->generateDotAll("myDot.dot");
|
||||
}
|
||||
|
||||
bool appl::InterfaceOutputManager::onTimer() {
|
||||
std::vector<std11::shared_ptr<appl::InterfaceOutputElement> >::iterator it = m_elementList.begin();
|
||||
etk::Vector<std11::shared_ptr<appl::InterfaceOutputElement> >::iterator it = m_elementList.begin();
|
||||
bool oneElementRemoved = false;
|
||||
while (it != m_elementList.end()) {
|
||||
if (*it == nullptr) {
|
||||
|
@ -16,19 +16,19 @@
|
||||
namespace appl {
|
||||
class InterfaceOutputManager {
|
||||
private:
|
||||
std::string m_name;
|
||||
etk::String m_name;
|
||||
public:
|
||||
const std::string& getName() {
|
||||
const etk::String& getName() {
|
||||
return m_name;
|
||||
}
|
||||
private:
|
||||
std11::shared_ptr<audio::river::Manager> m_manager;
|
||||
std::vector<std11::shared_ptr<appl::InterfaceOutputElement> > m_elementList;
|
||||
etk::Vector<std11::shared_ptr<appl::InterfaceOutputElement> > m_elementList;
|
||||
std11::mutex m_mutex;
|
||||
public:
|
||||
InterfaceOutputManager(const std::string& _name);
|
||||
InterfaceOutputManager(const etk::String& _name);
|
||||
~InterfaceOutputManager();
|
||||
void onTopicMessage(const std::string& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg);
|
||||
void onTopicMessage(const etk::String& _streamName, const audio_msg::AudioBuffer::ConstPtr& _msg);
|
||||
bool onTimer();
|
||||
};
|
||||
}
|
||||
|
@ -14,12 +14,12 @@
|
||||
namespace appl {
|
||||
int32_t getLogId();
|
||||
};
|
||||
// TODO : Review this problem of multiple intanciation of "std::stringbuf sb"
|
||||
// TODO : Review this problem of multiple intanciation of "etk::Stringbuf sb"
|
||||
#define APPL_BASE(info,data) \
|
||||
do { \
|
||||
if (info <= etk::log::getLevel(appl::getLogId())) { \
|
||||
std::stringbuf sb; \
|
||||
std::ostream tmpStream(&sb); \
|
||||
etk::Stringbuf sb; \
|
||||
etk::Stream tmpStream(&sb); \
|
||||
tmpStream << data; \
|
||||
etk::log::logStream(appl::getLogId(), info, __LINE__, __class__, __func__, tmpStream); \
|
||||
} \
|
||||
|
@ -38,7 +38,7 @@ bool f_create(audio_core::create::Request& _req,
|
||||
}
|
||||
_res.handle = newInterface->getId();
|
||||
mutex.lock();
|
||||
// TODO : g_listInterafceOut.push_back(newInterface);
|
||||
// TODO : g_listInterafceOut.pushBack(newInterface);
|
||||
mutex.unlock();
|
||||
APPL_INFO("create : [" << _res.handle << "] type: '" << _req.flowName << "'");
|
||||
*/
|
||||
@ -111,7 +111,7 @@ int main(int _argc, char **_argv) {
|
||||
ros::init(_argc, _argv, "audio_interface");
|
||||
etk::log::setLevel(etk::log::logLevelInfo);
|
||||
for (int32_t iii=0; iii<_argc ; ++iii) {
|
||||
std::string data = _argv[iii];
|
||||
etk::String data = _argv[iii];
|
||||
if (data == "-l0") {
|
||||
etk::log::setLevel(etk::log::logLevelNone);
|
||||
} else if (data == "-l1") {
|
||||
@ -147,11 +147,11 @@ int main(int _argc, char **_argv) {
|
||||
for (int32_t iii=0; iii<_argc; ++iii) {
|
||||
APPL_ERROR("Argument : " << iii << " '" << _argv[iii] << "'");
|
||||
}
|
||||
std::string hardwareInterface="DATA:hardware.json";
|
||||
etk::String hardwareInterface="DATA:hardware.json";
|
||||
for (int32_t iii=0; iii<_argc; ++iii) {
|
||||
std::string arg = _argv[iii];
|
||||
etk::String arg = _argv[iii];
|
||||
if (etk::start_with(_argv[iii], "--hardware=") == true) {
|
||||
hardwareInterface = std::string(&_argv[iii][11]);
|
||||
hardwareInterface = etk::String(&_argv[iii][11]);
|
||||
APPL_INFO("Find hardware configuration ... : '" << hardwareInterface << "'");
|
||||
}
|
||||
}
|
||||
|
@ -7,7 +7,7 @@
|
||||
#include <sstream>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
int64_t audioInterface_create(ros::NodeHandle& _n, ros::Time _timeUs, int32_t _freq, const std::vector<uint8_t> _channelMap) {
|
||||
int64_t audioInterface_create(ros::NodeHandle& _n, ros::Time _timeUs, int32_t _freq, const etk::Vector<uint8_t> _channelMap) {
|
||||
ros::ServiceClient client = _n.serviceClient<audio_core::create>("create");
|
||||
audio_core::create srv;
|
||||
srv.request.stamp = _timeUs;
|
||||
@ -48,14 +48,14 @@ uint32_t audioInterface_getBufferTime(ros::NodeHandle& _n, int64_t _uid) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
static std::string p_channelToPlay = "/audio/speaker";
|
||||
static etk::String p_channelToPlay = "/audio/speaker";
|
||||
static int32_t p_sampleRate = 48000;
|
||||
static int32_t p_nbChannels = 2;
|
||||
static int32_t p_frequency = 440;
|
||||
|
||||
static int32_t baseDataSize = 0;
|
||||
static double phase = 0.0;
|
||||
std::vector<uint8_t> channelMap;
|
||||
etk::Vector<uint8_t> channelMap;
|
||||
static ros::Publisher stream;
|
||||
|
||||
ros::Time nextFrame;
|
||||
@ -81,7 +81,7 @@ void onTimer(const ros::TimerEvent& _timer) {
|
||||
// Set the format of flow
|
||||
msg.channelFormat = audio_msg::AudioBuffer::FORMAT_INT16;
|
||||
|
||||
std::vector<int16_t> data;
|
||||
etk::Vector<int16_t> data;
|
||||
data.resize(baseDataSize*channelMap.size());
|
||||
double baseCycle = 2.0*M_PI/(double)p_sampleRate * (double)p_frequency;
|
||||
for (int32_t iii=0; iii<data.size()/channelMap.size(); iii++) {
|
||||
@ -149,19 +149,19 @@ int main(int _argc, char **_argv) {
|
||||
|
||||
|
||||
if (p_nbChannels == 1) {
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_CENTER);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_CENTER);
|
||||
} else if (p_nbChannels == 2) {
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_LEFT);
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_RIGHT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_LEFT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_RIGHT);
|
||||
} else if (p_nbChannels == 3) {
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_LEFT);
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_CENTER);
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_RIGHT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_LEFT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_CENTER);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_RIGHT);
|
||||
} else if (p_nbChannels == 4) {
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_LEFT);
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_FRONT_RIGHT);
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_REAR_LEFT);
|
||||
channelMap.push_back(audio_msg::AudioBuffer::CHANNEL_REAR_RIGHT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_LEFT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_FRONT_RIGHT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_REAR_LEFT);
|
||||
channelMap.pushBack(audio_msg::AudioBuffer::CHANNEL_REAR_RIGHT);
|
||||
} else {
|
||||
ROS_ERROR("nb chnnale supported error : %d not in [1,2,3,4]", p_nbChannels);
|
||||
exit(-1);
|
||||
|
@ -12,7 +12,7 @@ FILE* filee = NULL;
|
||||
void audioCallback(const audio_msg::AudioBuffer::ConstPtr& _msg) {
|
||||
audio::format format = audio::convertFormat(_msg->channelFormat);
|
||||
int32_t nbByteSample = audio::getFormatBytes(format);
|
||||
std::vector<enum audio::channel> channel = audio::convertChannel(_msg->channelMap);
|
||||
etk::Vector<enum audio::channel> channel = audio::convertChannel(_msg->channelMap);
|
||||
fwrite(&_msg->data[0], nbByteSample, _msg->data.size()/nbByteSample, filee);
|
||||
ROS_INFO_STREAM("get message: freq=" << _msg->frequency << " nbChannel=" << channel << " nbSample=" << _msg->data.size()/(_msg->channelMap.size()*nbByteSample) << " format=" << format);
|
||||
}
|
||||
@ -31,8 +31,8 @@ int main(int argc, char **argv) {
|
||||
ROS_ERROR ("not enought argument : %d", argc);
|
||||
usage();
|
||||
}
|
||||
std::string p_channelToRecord = argv[1];
|
||||
std::string p_fileToSave = argv[2];
|
||||
etk::String p_channelToRecord = argv[1];
|
||||
etk::String p_fileToSave = argv[2];
|
||||
|
||||
filee = fopen(p_fileToSave.c_str(), "w");
|
||||
ros::init(argc, argv, "test_audio_recorder");
|
||||
|
Loading…
Reference in New Issue
Block a user