YARP
Yet Another Robot Platform
Sound.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
7 #include <yarp/sig/Sound.h>
8 #include <yarp/sig/Image.h>
9 #include <yarp/os/Bottle.h>
10 #include <yarp/os/PortablePair.h>
11 #include <yarp/os/LogComponent.h>
12 #include <yarp/os/LogStream.h>
13 #include <yarp/os/Time.h>
14 #include <yarp/os/Value.h>
15 #include <functional>
16 
17 #include <cstring>
18 #include <cstdio>
19 
20 using namespace yarp::sig;
21 using namespace yarp::os;
22 
23 namespace {
24 YARP_LOG_COMPONENT(SOUND, "yarp.sig.Sound")
25 }
26 
27 #define HELPER(x) (*((FlexImage*)(x)))
28 
29 Sound::Sound(size_t bytesPerSample)
30 {
31  init(bytesPerSample);
32  m_frequency = 0;
33 }
34 
35 Sound::Sound(const Sound& alt) : yarp::os::Portable()
36 {
37  init(alt.getBytesPerSample());
38  FlexImage& img1 = HELPER(implementation);
39  FlexImage& img2 = HELPER(alt.implementation);
40  img1.copy(img2);
41  m_frequency = alt.m_frequency;
42  synchronize();
43 }
44 
46 {
47  if (alt.m_channels!= m_channels)
48  {
49  yCError(SOUND, "unable to concatenate sounds with different number of channels!");
50  return *this;
51  }
52  if (alt.m_frequency!= m_frequency)
53  {
54  yCError(SOUND, "unable to concatenate sounds with different sample rate!");
55  return *this;
56  }
57 
58  Sound orig= *this;
59  this->resize(this->m_samples+alt.m_samples,m_channels);
60 
61  unsigned char* p1 = orig.getRawData();
62  unsigned char* p2 = alt.getRawData();
63  unsigned char* pout = this->getRawData();
64 
65  for (size_t ch=0; ch<m_channels; ch++)
66  {
67  size_t out1 = ch* this->getBytesPerSample() * this->m_samples;
68  size_t out2 = ch* this->getBytesPerSample() * this->m_samples + this->getBytesPerSample() * orig.m_samples;
69 
70  size_t ori1 = ch * orig.getBytesPerSample() * orig.m_samples;
71  size_t s1 = orig.getBytesPerSample() * orig.m_samples;
72 
73  size_t alt1 = ch * orig.getBytesPerSample() * alt.m_samples;
74  unsigned int s2 = alt.getBytesPerSample() * alt.m_samples;
75 
76  memcpy((void *) &pout[out1], (void *) (p1+ori1), s1);
77  memcpy((void *) &pout[out2], (void *) (p2+alt1), s2);
78  }
79 
80  this->synchronize();
81  return *this;
82 }
83 
84 const Sound& Sound::operator = (const Sound& alt)
85 {
87  FlexImage& img1 = HELPER(implementation);
88  FlexImage& img2 = HELPER(alt.implementation);
89  img1.copy(img2);
90  m_frequency = alt.m_frequency;
91  synchronize();
92  return *this;
93 }
94 
95 void Sound::synchronize()
96 {
97  FlexImage& img = HELPER(implementation);
98  m_samples = img.width();
99  m_channels = img.height();
100 }
101 
102 Sound Sound::subSound(size_t first_sample, size_t last_sample)
103 {
104  if (last_sample > this->m_samples) {
105  last_sample = m_samples;
106  }
107  if (first_sample > this->m_samples) {
108  first_sample = m_samples;
109  }
110  if (last_sample < first_sample) {
111  last_sample = first_sample;
112  }
113 
114  Sound s;
115 
116  s.resize(last_sample-first_sample, this->m_channels);
117  s.setFrequency(this->m_frequency);
118 
119  /*
120  //faster implementation but currently not working
121  unsigned char* p1 = this->getRawData();
122  unsigned char* p2 = s.getRawData();
123  int j=0;
124  for (int i=first_sample; i<last_sample*2; i++)
125  {
126  p2[j++]=p1[i];
127  }
128  */
129 
130  //safe implementation
131  size_t j=0;
132  for (size_t i=first_sample; i<last_sample; i++)
133  {
134  for (size_t c = 0; c < this->m_channels; c++) {
135  s.set(this->get(i, c), j, c);
136  }
137  j++;
138  }
139 
140  s.synchronize();
141 
142  return s;
143 }
144 
145 void Sound::init(size_t bytesPerSample)
146 {
147  implementation = new FlexImage();
148  yCAssert(SOUND, implementation!=nullptr);
149 
150  yCAssert(SOUND, bytesPerSample==2); // that's all that's implemented right now
151  HELPER(implementation).setPixelCode(VOCAB_PIXEL_MONO16);
152  HELPER(implementation).setQuantum(2);
153 
154  m_samples = 0;
155  m_channels = 0;
156  this->m_bytesPerSample = bytesPerSample;
157 }
158 
160 {
161  if (implementation!=nullptr)
162  {
163  delete &HELPER(implementation);
164  implementation = nullptr;
165  }
166 }
167 
168 void Sound::resize(size_t samples, size_t m_channels)
169 {
170  FlexImage& img = HELPER(implementation);
171  img.resize(samples,m_channels);
172  synchronize();
173 }
174 
175 Sound::audio_sample Sound::get(size_t location, size_t channel) const
176 {
177  FlexImage& img = HELPER(implementation);
178  unsigned char *addr = img.getPixelAddress(location,channel);
179  if (m_bytesPerSample ==2)
180  {
181  return *(reinterpret_cast<NetUint16*>(addr));
182  }
183  else
184  {
185  yCError(SOUND, "sound only implemented for 16 bit samples");
186  }
187  return 0;
188 }
189 
191 {
192  size_t size = this->getRawDataSize();
193  unsigned char* p = this->getRawData();
194  memset(p,0,size);
195 }
196 
197 bool Sound::clearChannel(size_t chan)
198 {
199  if (chan > this->m_channels) {
200  return false;
201  }
202  for (size_t i = 0; i < this->m_samples; i++)
203  {
204  set(0, i, chan);
205  }
206  return true;
207 }
208 
209 void Sound::set(audio_sample value, size_t location, size_t channel)
210 {
211  FlexImage& img = HELPER(implementation);
212  unsigned char *addr = img.getPixelAddress(location,channel);
213  if (m_bytesPerSample ==2)
214  {
215  *(reinterpret_cast<NetUint16*>(addr)) = value;
216  return;
217  }
218  else
219  {
220  yCError(SOUND, "sound only implemented for 16 bit samples");
221  }
222 }
223 
225 {
226  return m_frequency;
227 }
228 
229 void Sound::setFrequency(int freq)
230 {
231  this->m_frequency = freq;
232 }
233 
234 bool Sound::read(ConnectionReader& connection)
235 {
236  // lousy format - fix soon!
237  FlexImage& img = HELPER(implementation);
238  Bottle bot;
239  bool ok = PortablePair<FlexImage,Bottle>::readPair(connection,img,bot);
240  m_frequency = bot.get(0).asInt32();
241  synchronize();
242  return ok;
243 }
244 
245 
246 bool Sound::write(ConnectionWriter& connection) const
247 {
248  // lousy format - fix soon!
249  FlexImage& img = HELPER(implementation);
250  Bottle bot;
251  bot.addInt32(m_frequency);
252  return PortablePair<FlexImage,Bottle>::writePair(connection,img,bot);
253 }
254 
255 unsigned char *Sound::getRawData() const
256 {
257  FlexImage& img = HELPER(implementation);
258  return img.getRawImage();
259 }
260 
261 size_t Sound::getRawDataSize() const
262 {
263  FlexImage& img = HELPER(implementation);
264  return img.getRawImageSize();
265 }
266 
267 void Sound::setSafe(audio_sample value, size_t sample, size_t channel)
268 {
269  if (isSample(sample, channel))
270  {
271  set(value, sample, channel);
272  }
273  else
274  {
275  yCError(SOUND) << "Sample out of bound:" << sample << "," << channel;
276  }
277 }
278 
279 Sound Sound::extractChannelAsSound(size_t channel_id) const
280 {
281  Sound news(this->m_bytesPerSample);
282  news.setFrequency(this->m_frequency);
283  news.resize(this->m_samples, 1);
284 
285  unsigned char* p_src = this->getRawData();
286  unsigned char* p_dst = news.getRawData();
287  size_t j = 0;
288  //pointer to the first element of the row of the matrix we need to copy
289  size_t first_sample = 0 + (this->m_samples * this->m_bytesPerSample)*channel_id;
290  //pointer to the last element of the row of the matrix we need to copy
291  size_t last_sample = first_sample + (this->m_samples * this->m_bytesPerSample);
292  for (auto i = first_sample; i < last_sample; i++)
293  {
294  p_dst[j++] = p_src[i];
295  }
296  return news;
297 }
298 
299 bool Sound::operator==(const Sound& alt) const
300 {
301  if (this->m_channels != alt.getChannels()) {
302  return false;
303  }
304  if (this->m_bytesPerSample != alt.getBytesPerSample()) {
305  return false;
306  }
307  if (this->m_frequency != alt.getFrequency()) {
308  return false;
309  }
310  if (this->m_samples != alt.getSamples()) {
311  return false;
312  }
313 
314  for (size_t ch = 0; ch < this->m_channels; ch++)
315  {
316  for (size_t s = 0; s < this->m_samples; s++)
317  {
318  if (this->getSafe(s, ch) != alt.getSafe(s, ch))
319  {
320  return false;
321  }
322  }
323  }
324 
325  return true;
326 }
327 
328 bool Sound::replaceChannel(size_t id, Sound schannel)
329 {
330  if (schannel.getChannels() != 1) {
331  return false;
332  }
333  if (this->m_samples != schannel.getSamples()) {
334  return false;
335  }
336  for (size_t s = 0; s < this->m_samples; s++)
337  {
338  this->setSafe(schannel.getSafe(s, 0), s, id);
339  }
340  return true;
341 }
342 
343 std::vector<std::reference_wrapper<Sound::audio_sample>> Sound::getChannel(size_t channel_id)
344 {
345  FlexImage& img = HELPER(implementation);
346 
347  std::vector<std::reference_wrapper<audio_sample>> vec;
348  vec.reserve(this->m_samples);
349  for (size_t t = 0; t < this->m_samples; t++)
350  {
351  unsigned char *addr = img.getPixelAddress(t, channel_id);
352  audio_sample* addr2 = reinterpret_cast<audio_sample*>(addr);
353  vec.push_back(std::ref(*addr2));
354  }
355  return vec;
356 }
357 
358 std::vector<std::reference_wrapper<Sound::audio_sample>> Sound::getInterleavedAudioRawData() const
359 {
360  FlexImage& img = HELPER(implementation);
361 
362  std::vector<std::reference_wrapper<audio_sample>> vec;
363  vec.reserve(this->m_samples*this->m_channels);
364  for (size_t t = 0; t < this->m_samples; t++)
365  {
366  for (size_t c = 0; c < this->m_channels; c++)
367  {
368  unsigned char *addr = img.getPixelAddress(t, c);
369  audio_sample* addr2 = reinterpret_cast<audio_sample*>(addr);
370  vec.push_back(std::ref(*addr2));
371  }
372  }
373  return vec;
374 }
375 
376 std::vector<std::reference_wrapper<Sound::audio_sample>> Sound::getNonInterleavedAudioRawData() const
377 {
378  FlexImage& img = HELPER(implementation);
379 
380  std::vector<std::reference_wrapper<audio_sample>> vec;
381  vec.reserve(this->m_samples*this->m_channels);
382  for (size_t c = 0; c < this->m_channels; c++)
383  {
384  for (size_t t = 0; t < this->m_samples; t++)
385  {
386  unsigned char *addr = img.getPixelAddress(t, c);
387  audio_sample* addr2 = reinterpret_cast<audio_sample*>(addr);
388  vec.push_back(std::ref(*addr2));
389  }
390  }
391  return vec;
392 }
393 
394 std::string Sound::toString() const
395 {
396  std::string s;
397  for (size_t c = 0; c < this->m_channels; c++)
398  {
399  for (size_t t = 0; t < this->m_samples; t++)
400  {
401  s += " ";
402  s += std::to_string(this->get(t, c));
403  }
404  s += '\n';
405  }
406  return s;
407 }
408 
409 bool Sound::isSample(size_t sample, size_t channel) const
410 {
411  return (sample<this->m_samples && channel<this->m_channels);
412 }
413 
415 {
416  return this->m_bytesPerSample;
417 }
418 
419 size_t Sound::getSamples() const
420 {
421  return this->m_samples;
422 }
423 
424 size_t Sound::getChannels() const
425 {
426  return this->m_channels;
427 }
428 
429 double Sound::getDuration() const
430 {
431  return (double)(this->m_samples)*(double)(1 / this->m_frequency);
432 }
433 
434 void Sound::normalizeChannel(size_t channel)
435 {
436  size_t maxsampleid = 0;
437  audio_sample maxsamplevalue = 0;
438  findPeakInChannel(channel, maxsampleid, maxsamplevalue);
439  double gain = 1 / (maxsamplevalue / 32767.0);
440  amplifyChannel(channel,gain);
441 }
442 
444 {
445  size_t maxsampleid = 0;
446  size_t maxchannelid = 0;
447  audio_sample maxsamplevalue = 0;
448  findPeak(maxchannelid, maxsampleid, maxsamplevalue);
449  double gain = 1 / (maxsamplevalue/32767.0);
450  amplify(gain);
451 }
452 
453 void Sound::amplifyChannel(size_t channel, double gain)
454 {
455  unsigned char* pc = this->getRawData();
456  audio_sample* p = reinterpret_cast<audio_sample*>(pc);
457  p+= this->m_samples * channel;
458 
459  for (size_t t = 0; t < this->m_samples; t++, p++)
460  {
461  double amplified_value = (*p) * gain;
462  *p = (int)(amplified_value); //should i limit this range
463  }
464 }
465 
466 void Sound::amplify(double gain)
467 {
468  for (size_t c = 0; c < this->m_channels; c++)
469  {
470  amplifyChannel(c,gain);
471  }
472 }
473 
474 void Sound::findPeakInChannel(size_t channelId, size_t& sampleId, audio_sample& sampleValue) const
475 {
476  sampleId = 0;
477  sampleValue = 0;
478  unsigned char* pc = this->getRawData();
479  audio_sample* p = reinterpret_cast<audio_sample*>(pc);
480  p += this->m_samples * channelId;
481 
482  for (size_t t = 0; t < this->m_samples; t++, p++)
483  {
484  if (*p > sampleValue)
485  {
486  sampleValue = (*p);
487  sampleId= t;
488  }
489  }
490 }
491 
492 void Sound::findPeak(size_t& channelId, size_t& sampleId, audio_sample& sampleValue) const
493 {
494  for (size_t c = 0; c < this->m_channels; c++)
495  {
496  size_t maxsampleid_inchannel=0;
497  audio_sample maxsamplevalue_inchannel=0;
498  findPeakInChannel(c, maxsampleid_inchannel, maxsamplevalue_inchannel);
499  if (maxsamplevalue_inchannel > sampleValue)
500  {
501  sampleValue = maxsamplevalue_inchannel;
502  sampleId = maxsampleid_inchannel;
503  channelId = c;
504  }
505  }
506 }
int16_t * samples
float t
#define HELPER(x)
Definition: Sound.cpp:27
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:74
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:140
An interface for reading from a network connection.
An interface for writing to a network connection.
Group a pair of objects to be sent and received together.
Definition: PortablePair.h:48
This is a base class for objects that can be both read from and be written to the YARP network.
Definition: Portable.h:26
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
Image class with user control of representation details.
Definition: Image.h:414
unsigned char * getPixelAddress(size_t x, size_t y) const
Get address of a pixel in memory.
Definition: Image.h:240
size_t width() const
Gets width of image in pixels.
Definition: Image.h:166
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition: Image.cpp:541
bool copy(const Image &alt)
Copy operator.
Definition: Image.cpp:836
size_t getRawImageSize() const
Access to the internal buffer size information (this is how much memory has been allocated for the im...
Definition: Image.cpp:550
void resize(size_t imgWidth, size_t imgHeight)
Reallocate an image to be of a desired size, throwing away its current contents.
Definition: Image.cpp:452
size_t height() const
Gets height of image in pixels.
Definition: Image.h:172
Class for storing sounds See Audio in YARP for additional documentation on YARP audio.
Definition: Sound.h:26
void setSafe(audio_sample value, size_t sample, size_t channel=0)
Definition: Sound.cpp:267
Sound extractChannelAsSound(size_t channel_id) const
Extract a single channel from the sound.
Definition: Sound.cpp:279
virtual ~Sound()
Definition: Sound.cpp:159
bool isSample(size_t sample, size_t channel=0) const
Check whether a sample lies within the sound.
Definition: Sound.cpp:409
bool clearChannel(size_t channel)
set to zero all the samples of the specified channel @ param channel the channel number
Definition: Sound.cpp:197
void setFrequency(int freq)
Set the frequency of the sound (i.e.
Definition: Sound.cpp:229
Sound subSound(size_t first_sample, size_t last_sample)
Returns a subpart of the sound.
Definition: Sound.cpp:102
std::vector< std::reference_wrapper< audio_sample > > getNonInterleavedAudioRawData() const
Returns a serialized version of the sound, in non-interleaved format, e.g.
Definition: Sound.cpp:376
const Sound & operator=(const Sound &alt)
Assignment operator.
Definition: Sound.cpp:84
void amplifyChannel(size_t channel, double gain)
Amplify a specific channel of the sound.
Definition: Sound.cpp:453
size_t getBytesPerSample() const
Get the number of bytes per sample.
Definition: Sound.cpp:414
short int audio_sample
Definition: Sound.h:28
std::vector< std::reference_wrapper< audio_sample > > getInterleavedAudioRawData() const
Returns a serialized version of the sound, in interleaved format, e.g.
Definition: Sound.cpp:358
double getDuration() const
Get the duration of sound in seconds.
Definition: Sound.cpp:429
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: Sound.cpp:234
size_t getChannels() const
Get the number of channels of the sound.
Definition: Sound.cpp:424
bool replaceChannel(size_t id, Sound channel)
Replace a single channel of our current sound with a given sound constituted by a single channel The ...
Definition: Sound.cpp:328
void normalize()
Normalize a sound (the peak is searched among all channels)
Definition: Sound.cpp:443
bool write(yarp::os::ConnectionWriter &connection) const override
Write this object to a network connection.
Definition: Sound.cpp:246
void amplify(double gain)
amplify a sound
Definition: Sound.cpp:466
void clear()
set all the samples to zero (silence)
Definition: Sound.cpp:190
Sound(size_t bytesPerSample=2)
Definition: Sound.cpp:29
void resize(size_t samples, size_t channels=1)
Set the sound size.
Definition: Sound.cpp:168
void normalizeChannel(size_t channel)
Normalize a specific channel of the sound.
Definition: Sound.cpp:434
void findPeak(size_t &channelId, size_t &sampleId, audio_sample &sampleValue) const
find the peak in a sound
Definition: Sound.cpp:492
int getFrequency() const
Get the frequency of the sound (i.e.
Definition: Sound.cpp:224
std::string toString() const
Print matrix to a string.
Definition: Sound.cpp:394
audio_sample get(size_t sample, size_t channel=0) const
Definition: Sound.cpp:175
std::vector< std::reference_wrapper< audio_sample > > getChannel(size_t channel_id)
Definition: Sound.cpp:343
Sound & operator+=(const Sound &alt)
Addition assignment operator.
Definition: Sound.cpp:45
bool operator==(const Sound &alt) const
Comparison operator.
Definition: Sound.cpp:299
void set(audio_sample value, size_t sample, size_t channel=0)
Definition: Sound.cpp:209
size_t getSamples() const
Get the number of samples contained in the sound.
Definition: Sound.cpp:419
audio_sample getSafe(size_t sample, size_t channel=0) const
Definition: Sound.h:80
void findPeakInChannel(size_t channelId, size_t &sampleId, audio_sample &sampleValue) const
find the peak in a specific channel of the sound
Definition: Sound.cpp:474
#define yCError(component,...)
Definition: LogComponent.h:154
#define yCAssert(component, x)
Definition: LogComponent.h:169
#define YARP_LOG_COMPONENT(name,...)
Definition: LogComponent.h:77
@ VOCAB_PIXEL_MONO16
Definition: Image.h:46
std::string to_string(IntegerType x)
Definition: numeric.h:115
An interface to the operating system, including Port based communication.
std::uint16_t NetUint16
Definition of the NetUint16 type.
Definition: NetUint16.h:30
Signal processing.
Definition: Image.h:22
The main, catch-all namespace for YARP.
Definition: dirs.h:16