diff --git a/audio_capture/launch/capture.launch b/audio_capture/launch/capture.launch index 6495bb21..5049ee1e 100644 --- a/audio_capture/launch/capture.launch +++ b/audio_capture/launch/capture.launch @@ -1,16 +1,19 @@ + like hw:1,0 + run pacmd list-sources to input specs + (but not all possible ones?) + --> - + - + diff --git a/audio_capture/scripts/audio_capture.py b/audio_capture/scripts/audio_capture.py new file mode 100755 index 00000000..19fdc8aa --- /dev/null +++ b/audio_capture/scripts/audio_capture.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python +# https://adnanalamkhan.wordpress.com/2015/03/01/using-gstreamer-1-0-with-python/ +import gi +# import rospy + +gi.require_version('Gst', '1.0') +# gi.require_version('Gtk', '3.0') +# from gi.repository import Gtk +from gi.repository import GObject +from gi.repository import Gst as gst + +GObject.threads_init() +gst.init(None) +# rospy.init_node('audio_capture') + +# Create the pipeline for our elements. +pipeline = gst.Pipeline() +# Create the elements for our project. + +audio_source = gst.ElementFactory.make('filesrc', 'audio_source') +# audio_source = gst.ElementFactory.make('alsasrc', 'audio_source') +decode = gst.ElementFactory.make('mad', 'decode') +convert = gst.ElementFactory.make('audioconvert', 'convert') +equalizer = gst.ElementFactory.make('equalizer-3bands', 'equalizer') +audio_sink = gst.ElementFactory.make('autoaudiosink', 'audio_sink') + +# Ensure all elements were created successfully. +if (not pipeline or not audio_source or not decode or + not convert or not equalizer or not audio_sink): + print('Not all elements could be created.') + exit(-1) + +# Configure our elements. +filename = 'blah' # 'Kevin_MacLeod_-_05_-_Impact_Allegretto.mp3' +audio_source.set_property('location', filename) +equalizer.set_property('band1', -24.0) +equalizer.set_property('band2', -24.0) + +# Add our elements to the pipeline. +pipeline.add(audio_source) +pipeline.add(decode) +pipeline.add(convert) +pipeline.add(equalizer) +pipeline.add(audio_sink) + +# Link our elements together. +audio_source.link(decode) +decode.link(convert) +convert.link(equalizer) +equalizer.link(audio_sink) + +# Set our pipelines state to Playing. +# check the following documentation whenever you get +# some AttributeError. +# link: http://lazka.github.io/pgi-docs/#Gst-1.0/flags.html +pipeline.set_state(gst.State.PLAYING) + +# Wait until error or EOS. +bus = pipeline.get_bus() + +while True: # not rospy.is_shutdown(): + # msg = bus.timed_pop_filtered(gst.CLOCK_TIME_NONE, gst.MessageType.ERROR | gst.MessageType.EOS) + msg = bus.timed_pop_filtered(1e9, gst.MessageType.ERROR | gst.MessageType.EOS) + if msg is None: + break + print msg + +# Free resources. +pipeline.set_state(gst.State.NULL) diff --git a/audio_capture/src/audio_capture.cpp b/audio_capture/src/audio_capture.cpp index 0ef67a87..593dc041 100644 --- a/audio_capture/src/audio_capture.cpp +++ b/audio_capture/src/audio_capture.cpp @@ -127,6 +127,22 @@ namespace audio_transport gst_bin_add_many( GST_BIN(_pipeline), _source, _filter, _sink, NULL); link_ok = gst_element_link_many( _source, _filter, _sink, NULL); } +#if 0 + GstCaps *caps; + // caps = gst_caps_new_simple("audio/x-raw-int", + caps = gst_caps_new_simple("audio/x-raw", + "channels", G_TYPE_INT, _channels, + "width", G_TYPE_INT, _depth, + "depth", G_TYPE_INT, _depth, + "rate", G_TYPE_INT, _sample_rate, + "signed", G_TYPE_BOOLEAN, TRUE, + NULL); + + g_object_set( G_OBJECT(_sink), "caps", caps, NULL); + gst_caps_unref(caps); + gst_bin_add_many( GST_BIN(_pipeline), _source, _sink, NULL); + link_ok = gst_element_link_many( _source, _sink, NULL); +#endif } else { ROS_ERROR_STREAM("format must be \"wave\" or \"mp3\""); exitOnMainThread(1); diff --git a/audio_play/src/audio_play.cpp b/audio_play/src/audio_play.cpp index fe127c2a..32fb3f78 100644 --- a/audio_play/src/audio_play.cpp +++ b/audio_play/src/audio_play.cpp @@ -140,7 +140,7 @@ namespace audio_transport gst_buffer_unref(buffer); } - static void cb_newpad (GstElement *decodebin, GstPad *pad, + static void cb_newpad (GstElement *decodebin, GstPad *pad, gpointer data) { RosGstPlay *client = reinterpret_cast(data); diff --git a/audio_to_float/.gitignore b/audio_to_float/.gitignore new file mode 100644 index 00000000..378eac25 --- /dev/null +++ b/audio_to_float/.gitignore @@ -0,0 +1 @@ +build diff --git a/audio_to_float/CHANGELOG.rst b/audio_to_float/CHANGELOG.rst new file mode 100644 index 00000000..f9093dbd --- /dev/null +++ b/audio_to_float/CHANGELOG.rst @@ -0,0 +1,88 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package audio_play +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Changed message level to warning +* Fixed problem that CMake uses gstreamer-0.1 instead of gstreamer-1.0 +* Fixed underflow. + Before the sink buffer underflows the pipeline is paused. When data is received again the pipeline is set to playing again. +* Added gstreamer 1.0 dependecies +* Ported to gstreamer 1.0 + package.xml dependencies still missing +* Contributors: Benny + +0.2.11 (2016-02-16) +------------------- +* Add changelogs +* Contributors: trainman419 + +0.2.10 (2016-01-21) +------------------- +* Add changelogs +* Contributors: trainman419 + +0.2.9 (2015-12-02) +------------------ +* Add changelogs +* Contributors: trainman419 + +0.2.8 (2015-10-02) +------------------ +* Changed message level to warning +* Fixed underflow. + Before the sink buffer underflows the pipeline is paused. When data is received again the pipeline is set to playing again. +* Change audio sink to autoaudiosink +* Update maintainer email +* Contributors: Benny, Hans Gaiser, trainman419 + +0.2.7 (2014-07-25) +------------------ + +0.2.6 (2014-02-26) +------------------ +* audio_capture and play _require\_ gstreamer, it's not optional +* Contributors: v4hn + +0.2.5 (2014-01-23) +------------------ +* "0.2.5" +* Contributors: trainman419 + +0.2.4 (2013-09-10) +------------------ + +0.2.3 (2013-07-15) +------------------ +* Fix dependencies and install rules. +* Contributors: Austin Hendrix + +0.2.2 (2013-04-10) +------------------ + +0.2.1 (2013-04-08 13:59) +------------------------ + +0.2.0 (2013-04-08 13:49) +------------------------ +* Finish catkinizing audio_common. +* Catkinize audio_play. +* Fix typo in package.xml +* Versions and more URLs. +* Convert manifests to package.xml +* Ditch old makefiles. +* Updates manifest +* Updated manifests for rodep2 +* oneiric build fixes, bump version to 0.1.6 +* Removed another duplicate thread::thread +* Added a rosdep.yaml file +* Fixed to use audio_common_msgs +* Added ability to use different festival voices +* Updated documentation +* Update to audio_play +* Fixed ignore files +* Added hgignore files +* Audio_capture and audio_play working +* Making separate audio_capture and audio_play packages +* Contributors: Austin Hendrix, Brian Gerkey, Nate Koenig, nkoenig diff --git a/audio_to_float/CMakeLists.txt b/audio_to_float/CMakeLists.txt new file mode 100644 index 00000000..0b091cb8 --- /dev/null +++ b/audio_to_float/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(audio_to_float) + +find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp audio_common_msgs) + +find_package(PkgConfig) +pkg_check_modules(GST1.0 gstreamer-1.0 REQUIRED) +pkg_check_modules(GSTAPP1.0 gstreamer-app-1.0 REQUIRED) + +find_package(Boost REQUIRED COMPONENTS thread) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${GST1.0_INCLUDE_DIRS} + ${GSTAPP1.0_INCLUDE_DIRS} +) + +catkin_package() + +add_executable(audio_to_float src/audio_to_float.cpp) +target_link_libraries(audio_to_float + ${catkin_LIBRARIES} + ${GST1.0_LIBRARIES} + ${GSTAPP1.0_LIBRARIES} + ${Boost_LIBRARIES} +) +add_dependencies(audio_to_float ${catkin_EXPORTED_TARGETS}) + +install(TARGETS audio_to_float + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/audio_to_float/launch/audio_to_float.launch b/audio_to_float/launch/audio_to_float.launch new file mode 100644 index 00000000..8454eb82 --- /dev/null +++ b/audio_to_float/launch/audio_to_float.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + diff --git a/audio_to_float/launch/spectrogram.launch b/audio_to_float/launch/spectrogram.launch new file mode 100644 index 00000000..db3f2c21 --- /dev/null +++ b/audio_to_float/launch/spectrogram.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/audio_to_float/mainpage.dox b/audio_to_float/mainpage.dox new file mode 100644 index 00000000..5c05407c --- /dev/null +++ b/audio_to_float/mainpage.dox @@ -0,0 +1,22 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b audio_play is a package that listens to a node that produces audio_msgs, and plays them through a connected speaker. + + +\section codeapi Code API + + + + +*/ diff --git a/audio_to_float/package.xml b/audio_to_float/package.xml new file mode 100644 index 00000000..c26a9151 --- /dev/null +++ b/audio_to_float/package.xml @@ -0,0 +1,32 @@ + + audio_to_float + 0.2.7 + + Converts a stream of gstreamer AudioData messages to a stream of floating point arrays. + + Lucas Walter + Lucas Walter + BSD + http://ros.org/wiki/audio_play + https://github.com/ros-drivers/audio_common + https://github.com/ros-drivers/audio_common/issues + + catkin + + cv_bridge + roscpp + audio_common_msgs + libgstreamer1.0-dev + libgstreamer-plugins-base1.0-dev + + cv_bridge + roscpp + audio_common_msgs + libgstreamer1.0-0 + libgstreamer-plugins-base1.0-0 + gstreamer1.0-plugins-ugly + gstreamer1.0-plugins-good + + + + diff --git a/audio_to_float/scripts/spectrogram.py b/audio_to_float/scripts/spectrogram.py new file mode 100755 index 00000000..575d278c --- /dev/null +++ b/audio_to_float/scripts/spectrogram.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +import collections +import numpy as np +import rospy + +# from audio_common_msgs.msg import AudioData +from cv_bridge import CvBridge +from scipy import signal +from sensor_msgs.msg import ChannelFloat32, Image + + +class View(): + def __init__(self): + self.bridge = CvBridge() + self.buffer_len = rospy.get_param("~buffer_len", 2**16) + self.buffer = collections.deque(maxlen=self.buffer_len) + self.sample_rate = rospy.get_param("~sample_rate", 44100) + # self.window = 256 + self.im = None + self.pub = rospy.Publisher("image_spectrogram", Image, queue_size=1) + self.sub = rospy.Subscriber("samples", ChannelFloat32, + self.audio_callback, queue_size=1) + self.timer = rospy.Timer(rospy.Duration(0.2), self.update) + + def audio_callback(self, msg): + for i in range(len(msg.values)): + self.buffer.append(msg.values[i]) + + def update(self, event): + if len(self.buffer) < self.buffer_len: + return + samples = np.asarray(self.buffer) + # TODO(lucasw) this is hugely inefficient if it is re-calculating + # for samples that were processed in previous update. + f, t, Sxx = signal.spectrogram(samples, self.sample_rate, nperseg=512) + # TODO(lucasw) is there a standard spectrogram conversion? + Sxx = np.log(1.0 + Sxx * 2**16) + mins = np.min(Sxx) + maxs = np.max(Sxx) + Sxx -= mins + print(Sxx.shape, mins, maxs) + self.im = (Sxx * 50).astype(np.uint8) + # self.im[y0:y1+1, i, :] = 255 + self.pub.publish(self.bridge.cv2_to_imgmsg(self.im, "mono8")) + # rospy.signal_shutdown("") + + +if __name__ == '__main__': + rospy.init_node('spectrogram') + view = View() + rospy.spin() diff --git a/audio_to_float/scripts/view.py b/audio_to_float/scripts/view.py new file mode 100755 index 00000000..e97b9e82 --- /dev/null +++ b/audio_to_float/scripts/view.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python + +import collections +import numpy as np +import rospy + +# from audio_common_msgs.msg import AudioData +from cv_bridge import CvBridge +from sensor_msgs.msg import ChannelFloat32, Image + + +class View(): + def __init__(self): + self.bridge = CvBridge() + self.fade1 = rospy.get_param("~fade1", 0.9) + self.fade2 = rospy.get_param("~fade2", 0.99) + self.buffer = collections.deque(maxlen=8192) + self.im = np.zeros((256, 1300, 3), np.uint8) + self.pub = rospy.Publisher("image", Image, queue_size=1) + self.sub = rospy.Subscriber("decoded", ChannelFloat32, + self.audio_callback, queue_size=1) + self.timer = rospy.Timer(rospy.Duration(0.05), self.update) + + def audio_callback(self, msg): + for i in range(len(msg.values)): + self.buffer.append(msg.values[i]) + + def update(self, event): + self.im[:, :, 1:3] = (self.im[:, :, 1:3] * self.fade1).astype(np.uint8) + self.im[:, :, 0] = (self.im[:, :, 0] * self.fade2).astype(np.uint8) + width = self.im.shape[1] + height = self.im.shape[0] + last_y = 0 + for i in range(0, width): + if i >= len(self.buffer): + break + sample = self.buffer[i] + sample *= height / 2 + sample += height / 2 + y = int(sample) % height + y0 = min(last_y, y) + y1 = max(last_y, y) + self.im[y0:y1 + 1, i, :] = 255 + last_y = y + self.pub.publish(self.bridge.cv2_to_imgmsg(self.im, "bgr8")) + + +if __name__ == '__main__': + rospy.init_node('view_audio') + view = View() + rospy.spin() diff --git a/audio_to_float/src/audio_to_float.cpp b/audio_to_float/src/audio_to_float.cpp new file mode 100644 index 00000000..2844349c --- /dev/null +++ b/audio_to_float/src/audio_to_float.cpp @@ -0,0 +1,246 @@ +#include +#include +#include +#include +#include + +#include "audio_common_msgs/AudioData.h" +#include "sensor_msgs/ChannelFloat32.h" + +namespace audio_transport +{ + class RosGstToFloat + { + public: + RosGstToFloat() + { + GstPad *audiopad; + + int output_sample_rate; + ros::param::param("~output_sample_rate", output_sample_rate, 16000); + + _sub = _nh.subscribe("audio", 10, &RosGstToFloat::onAudio, this); + _pub = _nh.advertise("decoded", 10); + + _loop = g_main_loop_new(NULL, false); + + _pipeline = gst_pipeline_new("app_pipeline"); + _source = gst_element_factory_make("appsrc", "app_source"); + gst_bin_add( GST_BIN(_pipeline), _source); + + g_signal_connect(_source, "need-data", G_CALLBACK(cb_need_data),this); + + // http://stackoverflow.com/questions/35310415/how-to-access-data-from-gmemoryoutputstream + // https://github.com/jojva/gst-plugins-base/blob/master/tests/examples/app/appsink-src.c + { + _decoder = gst_element_factory_make("decodebin", "decoder"); + if (_decoder == NULL) + { + ROS_ERROR_STREAM("couldn't create _sink"); + return; + } + g_signal_connect(_decoder, "pad-added", G_CALLBACK(cb_newpad),this); + gst_bin_add( GST_BIN(_pipeline), _decoder); + gst_element_link(_source, _decoder); + + _audio = gst_bin_new("audiobin"); + if (_audio == NULL) + { + ROS_ERROR_STREAM("couldn't create _sink"); + return; + } + _convert = gst_element_factory_make("audioconvert", "convert"); + if (_convert == NULL) + { + ROS_ERROR_STREAM("couldn't create _sink"); + return; + } + _resample = gst_element_factory_make("audioresample", "resample"); + if (_resample == NULL) + { + ROS_ERROR_STREAM("couldn't create _sink"); + return; + } + // TODO(lucasw) what is this for? + audiopad = gst_element_get_static_pad(_convert, "sink"); + _sink = gst_element_factory_make("appsink", "sink"); + if (_sink == NULL) + { + ROS_ERROR_STREAM("couldn't create _sink"); + return; + } + + { + // 'caps' -> capabilities + GstCaps *caps; + caps = gst_caps_new_simple("audio/x-raw", + "format", G_TYPE_STRING, "F32LE", + "channels", G_TYPE_INT, 1, + "layout", G_TYPE_STRING, "interleaved", + "channel-mask", GST_TYPE_BITMASK, 0x0000000000000001, + // "width - bits per sample + // depth - bits ACTUALLY USED FOR AUDIO per sample + // You can have 32-bit samples, but in each + // 32-bit group only 16 or 24 bits + // will be used. + // This is to achieve necessary alignment." + "width", G_TYPE_INT, 32, + "depth", G_TYPE_INT, 32, + "endianness", G_TYPE_INT, G_BYTE_ORDER, // 1234 + "rate", G_TYPE_INT, output_sample_rate, + "signed", G_TYPE_BOOLEAN, TRUE, + NULL); + g_object_set( G_OBJECT(_sink), "caps", caps, NULL); + gst_caps_unref(caps); + } + + g_object_set (G_OBJECT (_sink), "emit-signals", TRUE, "sync", FALSE, NULL); + g_signal_connect (_sink, "new-sample", + G_CALLBACK (on_new_sample_from_sink), this); + + gst_bin_add_many( GST_BIN(_audio), _resample, _convert, _sink, NULL); + // gst_bin_add_many( GST_BIN(_audio), _convert, _sink, NULL); + gst_element_link(_convert, _sink); + gst_element_add_pad(_audio, gst_ghost_pad_new("sink", audiopad)); + gst_object_unref(audiopad); + + gst_bin_add(GST_BIN(_pipeline), _audio); + } + + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); + + _gst_thread = boost::thread( boost::bind(g_main_loop_run, _loop) ); + + _paused = false; + } + + private: + + void onAudio(const audio_common_msgs::AudioDataConstPtr &msg) + { + // ROS_DEBUG_STREAM("new audio " << msg->data.size()); + if(_paused) + { + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); + _paused = false; + } + + GstBuffer *buffer = gst_buffer_new_and_alloc(msg->data.size()); + gst_buffer_fill(buffer, 0, &msg->data[0], msg->data.size()); + GstFlowReturn ret; + + g_signal_emit_by_name(_source, "push-buffer", buffer, &ret); + } + + /* called when the appsink notifies us that there is a new buffer ready for + o/ http://fossies.org/linux/gst-plugins-base/tests/examples/app/appsink-src.c + * processing */ + static void // GstFlowReturn + on_new_sample_from_sink (GstElement * elt, gpointer data) + { + ROS_DEBUG_STREAM("new sample"); + RosGstToFloat *client = reinterpret_cast(data); + GstSample *sample; + GstBuffer *buffer; + GstElement *source; + + /* get the sample from appsink */ + sample = gst_app_sink_pull_sample (GST_APP_SINK (elt)); + buffer = gst_sample_get_buffer (sample); + + GstMapInfo map; + if (gst_buffer_map (buffer, &map, GST_MAP_READ)) + { + // ROS_INFO_STREAM("map " + // << map.size << " " + // << map.maxsize << " "); + // gst_util_dump_mem (map.data, map.size); + sensor_msgs::ChannelFloat32 msg; + const size_t sz = sizeof(float); + msg.values.resize(map.size / sz); + // TODO(lucasw) copy this more efficiently + for (size_t i = 0; i < map.size / sz; ++i) + { + // TODO(lucasw) can this format be assumed from + // default conversion settings here? + #if 0 + const int hi = (map.data[i * 2 + 1] + 128) % 256; + // TODO(lucasw) not sure about the + 128 here + const int lo = (map.data[i * 2] + 128) % 256; + msg.values[i] = (static_cast(hi - 127) * 256.0 + lo) / + static_cast(1 << 15); + #endif + msg.values[i] = *(reinterpret_cast(&map.data[i * sz])); + } + gst_buffer_unmap (buffer, &map); + client->_pub.publish(msg); + } + gst_sample_unref (sample); + } + + static void cb_newpad (GstElement *decodebin, GstPad *pad, + gpointer data) + { + RosGstToFloat *client = reinterpret_cast(data); + + GstCaps *caps; + GstStructure *str; + GstPad *audiopad; + + /* only link once */ + audiopad = gst_element_get_static_pad (client->_audio, "sink"); + if (GST_PAD_IS_LINKED (audiopad)) + { + g_object_unref (audiopad); + return; + } + + /* check media type */ + caps = gst_pad_query_caps (pad, NULL); + str = gst_caps_get_structure (caps, 0); + if (!g_strrstr (gst_structure_get_name (str), "audio")) { + gst_caps_unref (caps); + gst_object_unref (audiopad); + return; + } + + gst_caps_unref (caps); + + /* link'n'play */ + gst_pad_link (pad, audiopad); + + g_object_unref (audiopad); + } + + static void cb_need_data (GstElement *appsrc, + guint unused_size, + gpointer user_data) + { + ROS_DEBUG("need-data signal emitted! Pausing the pipeline"); + RosGstToFloat *client = reinterpret_cast(user_data); + gst_element_set_state(GST_ELEMENT(client->_pipeline), GST_STATE_PAUSED); + client->_paused = true; + } + + ros::NodeHandle _nh; + ros::Subscriber _sub; + ros::Publisher _pub; + boost::thread _gst_thread; + + GstElement *_pipeline, *_source, *_sink, *_decoder, *_convert, *_resample, *_audio; + GMainLoop *_loop; + + bool _paused; + }; +} + + +int main (int argc, char **argv) +{ + ros::init(argc, argv, "audio_play"); + gst_init(&argc, &argv); + + audio_transport::RosGstToFloat client; + + ros::spin(); +} diff --git a/float_to_audio/.gitignore b/float_to_audio/.gitignore new file mode 100644 index 00000000..378eac25 --- /dev/null +++ b/float_to_audio/.gitignore @@ -0,0 +1 @@ +build diff --git a/float_to_audio/CMakeLists.txt b/float_to_audio/CMakeLists.txt new file mode 100644 index 00000000..7f2fd3e2 --- /dev/null +++ b/float_to_audio/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(float_to_audio) + +find_package(catkin REQUIRED COMPONENTS roscpp audio_common_msgs) + +find_package(PkgConfig) +pkg_check_modules(GST1.0 gstreamer-1.0 REQUIRED) + +find_package(Boost REQUIRED COMPONENTS thread) + +include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GST1.0_INCLUDE_DIRS}) + +catkin_package() + +add_executable(float_to_audio src/float_to_audio.cpp) +target_link_libraries(float_to_audio ${catkin_LIBRARIES} ${GST1.0_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(float_to_audio ${catkin_EXPORTED_TARGETS}) + +install(TARGETS float_to_audio + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/float_to_audio/config/float_to_audio.perspective b/float_to_audio/config/float_to_audio.perspective new file mode 100644 index 00000000..aef02c20 --- /dev/null +++ b/float_to_audio/config/float_to_audio.perspective @@ -0,0 +1,142 @@ +{ + "keys": {}, + "groups": { + "pluginmanager": { + "keys": { + "running-plugins": { + "type": "repr", + "repr": "{u'rqt_image_view/ImageView': [2, 1]}" + } + }, + "groups": { + "plugin__rqt_image_view__ImageView__2": { + "keys": {}, + "groups": { + "dock_widget__ImageViewWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Image View (2)'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "max_range": { + "type": "repr", + "repr": "10.0" + }, + "mouse_pub_topic": { + "type": "repr", + "repr": "u'/audio/image_output_mouse_left'" + }, + "zoom1": { + "type": "repr", + "repr": "True" + }, + "dynamic_range": { + "type": "repr", + "repr": "False" + }, + "topic": { + "type": "repr", + "repr": "u'/audio/image_output'" + }, + "publish_click_location": { + "type": "repr", + "repr": "False" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_image_view__ImageView__1": { + "keys": {}, + "groups": { + "dock_widget__ImageViewWidget": { + "keys": { + "dockable": { + "type": "repr", + "repr": "True" + }, + "parent": { + "type": "repr", + "repr": "None" + }, + "dock_widget_title": { + "type": "repr", + "repr": "u'Image View'" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "max_range": { + "type": "repr", + "repr": "10.0" + }, + "mouse_pub_topic": { + "type": "repr", + "repr": "u'/audio/image_input_mouse_left'" + }, + "zoom1": { + "type": "repr", + "repr": "True" + }, + "dynamic_range": { + "type": "repr", + "repr": "False" + }, + "topic": { + "type": "repr", + "repr": "u'/audio/image_input'" + }, + "publish_click_location": { + "type": "repr", + "repr": "False" + } + }, + "groups": {} + } + } + } + } + }, + "mainwindow": { + "keys": { + "geometry": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00020000000000e5000000330000061c0000035a000000e60000004f0000061b0000035900000000000000000780')", + "pretty-print": " 3 Z O Y " + }, + "state": { + "type": "repr(QByteArray.hex)", + "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd000000010000000300000536000002dcfc0100000001fc00000000000005360000052800000528fc0200000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d0061006700650056006900650077005700690064006700650074010000001a0000016b0000016b0000016bfb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d006100670065005600690065007700570069006400670065007401000001870000016b0000016b0000016b000005280000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "pretty-print": " 6 ( ( k k k k k k ( " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "type": "repr", + "repr": "8" + } + }, + "groups": {} + } + } + } + } +} \ No newline at end of file diff --git a/float_to_audio/launch/audio_to_float_to_audio.launch b/float_to_audio/launch/audio_to_float_to_audio.launch new file mode 100644 index 00000000..b52d3f76 --- /dev/null +++ b/float_to_audio/launch/audio_to_float_to_audio.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/float_to_audio/launch/float_to_audio.launch b/float_to_audio/launch/float_to_audio.launch new file mode 100644 index 00000000..0bad48a4 --- /dev/null +++ b/float_to_audio/launch/float_to_audio.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/float_to_audio/package.xml b/float_to_audio/package.xml new file mode 100644 index 00000000..64d19a4c --- /dev/null +++ b/float_to_audio/package.xml @@ -0,0 +1,27 @@ + + float_to_audio + 0.2.7 + + Convert a float array topic into gstreamer audio message that can be played by audio_play. + + Lucas Walter + Lucas Walter + BSD + http://ros.org/wiki/float_to_audio + https://github.com/ros-drivers/audio_common + https://github.com/ros-drivers/audio_common/issues + + catkin + + roscpp + audio_common_msgs + libgstreamer1.0-dev + libgstreamer-plugins-base1.0-dev + + roscpp + audio_common_msgs + libgstreamer1.0-0 + libgstreamer-plugins-base1.0-0 + gstreamer1.0-plugins-ugly + gstreamer1.0-plugins-good + diff --git a/float_to_audio/scripts/gen_float.py b/float_to_audio/scripts/gen_float.py new file mode 100755 index 00000000..a080bc5a --- /dev/null +++ b/float_to_audio/scripts/gen_float.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python +# Lucas Walter +# Generate a series of float arrays where number of samples * array rate = sample rate +# (e.g 1000 samples/msg * 16 msgs/sec = 16 KHz) +# +# TODO(lucasw) +# Make this node generate a constant stream, but default to zero +# then have it subscribe to an input topic that accepts arbitrary length +# float arrays, it will mix these into the other stream as it receives them +# receiving a high rate of incoming messages will cause them all to be summed +# together. +# In order to support this a fifo buffer will store all the samples to be +# played back, and grow it as needed to support the longest sample received. + +import random +import rospy + +from sensor_msgs.msg import ChannelFloat32 + + +class GenFloat(): + def __init__(self): + self.msg_rate = rospy.get_param("~msg_rate", 16) + self.sample_rate = rospy.get_param("~sample_rate", 16000) + self.samples_per_msg = int(self.sample_rate / self.msg_rate) + # not quite 1.0 / self.msg_rate + self.dt = float(self.samples_per_msg) / float(self.sample_rate) + rospy.loginfo(self.dt) + + self.msg = ChannelFloat32() + for i in range(self.samples_per_msg): + self.msg.values.append(0) + self.counter = int(0) + + # signal period for each 'voice'/channel + self.period = [200, 417, 400] + # in samples + self.phase = [0, 0, 0] + self.pub = rospy.Publisher("samples", ChannelFloat32, queue_size=10) + self.timer = rospy.Timer(rospy.Duration(self.dt), self.update) + + def update(self, event): + for i in range(self.samples_per_msg): + val = 0 + # square wave + sc = 0.2 + ind = 0 + if self.counter % self.period[ind] < self.period[ind] / 2: + val += sc + else: + val -= sc + # triangle + sc = 0.3 + ind = 1 + fr = float(self.counter % self.period[ind]) / float(self.period[ind]) + if fr < 0.5: + val += sc * (fr * 4.0 - 1.0) + else: + val += sc * (1.0 - ((fr - 0.5) * 4.0)) + val += random.random() * 0.2 + self.msg.values[i] = val + self.counter += 1 + + self.pub.publish(self.msg) + + +if __name__ == '__main__': + rospy.init_node('gen_float') + gen_float = GenFloat() + rospy.spin() diff --git a/float_to_audio/scripts/mixer.py b/float_to_audio/scripts/mixer.py new file mode 100755 index 00000000..edd1996f --- /dev/null +++ b/float_to_audio/scripts/mixer.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python +# Lucas Walter +# Subscribe to ChannelFloat32 topic and mix together messages to provide a +# fixed sample rate output ChannelFloat32 suitable for conversion to audio +# in a float_to_audio node. +# Generate silence as needed when there are no incoming samples diff --git a/float_to_audio/scripts/noise.py b/float_to_audio/scripts/noise.py new file mode 100755 index 00000000..47fb06c7 --- /dev/null +++ b/float_to_audio/scripts/noise.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python +# Lucas Walter +# NES style pseudo random 1-bit noise +# http://wiki.nesdev.com/w/index.php/APU_Noise + +import rospy +from sensor_msgs.msg import ChannelFloat32 +from std_msgs.msg import Float32 + + +class Noise(): + def __init__(self): + self.shift_register = 0x0001 + self.msg_rate = rospy.get_param("~msg_rate", 8) + self.sample_rate = rospy.get_param("~sample_rate", 16000) + self.samples_per_msg = int(self.sample_rate / self.msg_rate) + self.dt = float(self.samples_per_msg) / float(self.sample_rate) + self.msg = ChannelFloat32() + for i in range(self.samples_per_msg): + self.msg.values.append(0) + self.counter = int(0) + # frequency range should be from 29.3 Hz to 447 KHz + self.frequency = 500 + self.frequency_sub = rospy.Subscriber("frequency", Float32, + self.frequency_callback, queue_size=1) + self.pub = rospy.Publisher("samples", ChannelFloat32, queue_size=10) + self.timer = rospy.Timer(rospy.Duration(self.dt), self.update) + + def frequency_callback(self, msg): + self.frequency = msg.data + + def update(self, event): + period = int(self.sample_rate / self.frequency) + + sc = 0.4 + loop = True + cycles = 0 + while loop: + feedback = (self.shift_register & 0x1 > 0) != (self.shift_register & (1 << 6) > 0) + # feedback = (self.shift_register & 0x1 > 0) != (self.shift_register & 0x2 > 0) + # print self.counter, cycles, self.shift_register & 0x1, format(self.shift_register, "015b") + self.shift_register = self.shift_register >> 1 + self.shift_register |= feedback << 14 + for j in range(self.counter % period, period): + ind = cycles * period + j + if ind >= self.samples_per_msg: + loop = False + break + if not (self.shift_register & 0x1): + self.msg.values[ind] = sc + else: + self.msg.values[ind] = -sc + self.counter += 1 + cycles += 1 + # print ("%s" % (format(self.shift_register, "04x"))) + self.pub.publish(self.msg) + # rospy.signal_shutdown("debug quit") + + +if __name__ == '__main__': + rospy.init_node('noise') + noise = Noise() + rospy.spin() diff --git a/float_to_audio/scripts/sample.py b/float_to_audio/scripts/sample.py new file mode 100755 index 00000000..0f7f3c98 --- /dev/null +++ b/float_to_audio/scripts/sample.py @@ -0,0 +1,6 @@ +#!/usr/bin/env python +# Lucas Walter +# When trigger record a ChannelFloat32 stream, appending the sequence of messages to +# a single ChannelFloat32 as need until recording is stopped. +# Then when trigger play the sample on an output ChannelFloat32 topic, optionally +# resample it to play at different rates. diff --git a/float_to_audio/src/float_to_audio.cpp b/float_to_audio/src/float_to_audio.cpp new file mode 100644 index 00000000..08bf7387 --- /dev/null +++ b/float_to_audio/src/float_to_audio.cpp @@ -0,0 +1,314 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +// rostopic pub /samples sensor_msgs/ChannelFloat32 "{name: '', values: [-0.4, 0.0]}" +// GST_DEBUG="*:5" rosrun float_to_audio float_to_audio + +// gst-launch-1.0 audiotestsrc ! audio/x-raw, format="F32LE", rate=16000 ! audioconvert ! audioresample ! audio/x-raw, format="S16LE", rate=8000 ! autoaudiosink + + +namespace audio_transport +{ + class RosFloatToGst + { + public: + RosFloatToGst() + { + // Need to encoding or publish raw wave data + ros::param::param("~format", _format, "mp3"); + // The bitrate at which to encode the audio + ros::param::param("~bitrate", _bitrate, 192); + // only available for raw data + ros::param::param("~channels", _channels, 1); + ros::param::param("~depth", _depth, 16); + ros::param::param("~sample_rate", _sample_rate, 16000); + + int input_sample_rate; + ros::param::param("~input_sample_rate", input_sample_rate, 16000); + + _pub = _nh.advertise("audio", 10, true); + + _loop = g_main_loop_new(NULL, false); + _pipeline = gst_pipeline_new("ros_pipeline"); + _bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline)); + gst_bus_add_signal_watch(_bus); + g_signal_connect(_bus, "message::error", + G_CALLBACK(onMessage), this); + g_object_unref(_bus); + + // We create the sink first, just for convenience + _sink = gst_element_factory_make("appsink", "sink"); + if (_sink == NULL) + { + ROS_ERROR_STREAM("couldn't create sink"); + exitOnMainThread(1); + } + g_object_set(G_OBJECT(_sink), "emit-signals", true, NULL); + g_object_set(G_OBJECT(_sink), "max-buffers", 100, NULL); + g_signal_connect( G_OBJECT(_sink), "new-sample", + G_CALLBACK(onNewBuffer), this); + + _audioresample = gst_element_factory_make("audioresample", "audioresample"); + if (_audioresample == NULL) + { + ROS_ERROR_STREAM("couldn't create audioresample"); + exitOnMainThread(1); + } + + _audioconvert = gst_element_factory_make("audioconvert", "audioconvert"); + if (_audioconvert == NULL) + { + ROS_ERROR_STREAM("couldn't create audioconvert"); + exitOnMainThread(1); + } + + _source = gst_element_factory_make("appsrc", "source"); + if (_source == NULL) + { + ROS_ERROR_STREAM("couldn't create source"); + exitOnMainThread(1); + } + g_signal_connect(_source, "need-data", G_CALLBACK(cb_need_data),this); + + { + // 'caps' -> capabilities + GstCaps *caps; + caps = gst_caps_new_simple("audio/x-raw", + "format", G_TYPE_STRING, "F32LE", + "channels", G_TYPE_INT, 1, + "layout", G_TYPE_STRING, "interleaved", + "channel-mask", GST_TYPE_BITMASK, 0x0000000000000001, + // "width - bits per sample + // depth - bits ACTUALLY USED FOR AUDIO per sample + // You can have 32-bit samples, but in each + // 32-bit group only 16 or 24 bits + // will be used. + // This is to achieve necessary alignment." + "width", G_TYPE_INT, 32, + "depth", G_TYPE_INT, 32, + "endianness", G_TYPE_INT, G_BYTE_ORDER, // 1234 + "rate", G_TYPE_INT, input_sample_rate, + "signed", G_TYPE_BOOLEAN, TRUE, + NULL); + g_object_set( G_OBJECT(_source), "caps", caps, NULL); + gst_caps_unref(caps); + } + + // mp3 + // audio/x-raw, format=(string)S16LE, layout=(string)interleaved, + // rate=(int){ 8000, 11025, 12000, 16000, 22050, 24000, 32000, 44100, 48000 }, + // channels=(int)1 + + gboolean link_ok; + if (_format == "mp3"){ + _encode = gst_element_factory_make("lamemp3enc", "encoder"); + if (_encode == NULL) + { + ROS_ERROR_STREAM("couldn't create encode"); + exitOnMainThread(1); + } + g_object_set( G_OBJECT(_encode), "quality", 2.0, NULL); + g_object_set( G_OBJECT(_encode), "bitrate", _bitrate, NULL); + + link_ok = addAllToPipeline(); + } + else if (_format == "flac") + { + _encode = gst_element_factory_make("flacenc", "encoder"); + link_ok = addAllToPipeline(); + #if 0 + } else if (_format == "wave") { + // TODO(lwalter) this isn't working + GstCaps *caps; + caps = gst_caps_new_simple("audio/x-raw", + "format", G_TYPE_STRING, "S16LE", + "channels", G_TYPE_INT, _channels, + // // "layout", G_TYPE_INT, GST_AUDIO_LAYOUT_INTERLEAVED, + "width", G_TYPE_INT, _depth, + "depth", G_TYPE_INT, _depth, + "rate", G_TYPE_INT, _sample_rate, + "signed", G_TYPE_BOOLEAN, TRUE, + NULL); + + g_object_set( G_OBJECT(_sink), "caps", caps, NULL); + gst_caps_unref(caps); + + gst_bin_add_many(GST_BIN(_pipeline), _source, _audioconvert, + _audioresample, _sink, NULL); + link_ok = gst_element_link_many(_source, _audioconvert, + _audioresample, _sink, NULL); + #endif + } else { + ROS_ERROR_STREAM("format must be \"flac\" or \"mp3\""); + exitOnMainThread(1); + } + + if (!link_ok) { + ROS_ERROR_STREAM("Unsupported media type."); + exitOnMainThread(1); + } + + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); + + _sub = _nh.subscribe("samples", 10, &RosFloatToGst::onFloat, this); + _gst_thread = boost::thread( boost::bind(g_main_loop_run, _loop) ); + _paused = false; + } + + ~RosFloatToGst() + { + g_main_loop_quit(_loop); + gst_element_set_state(_pipeline, GST_STATE_NULL); + gst_object_unref(_pipeline); + g_main_loop_unref(_loop); + } + + void exitOnMainThread(int code) + { + ros::shutdown(); + exit(code); + } + + gboolean addAllToPipeline() + { + if (!gst_bin_add( GST_BIN(_pipeline), _source)) + { + ROS_ERROR_STREAM("source"); + return false; + } + if (!gst_bin_add( GST_BIN(_pipeline), _audioconvert)) + { + ROS_ERROR_STREAM("audioconvert"); + return false; + } + if (!gst_bin_add( GST_BIN(_pipeline), _audioresample)) + { + ROS_ERROR_STREAM("audioresample"); + return false; + } + if (!gst_bin_add( GST_BIN(_pipeline), _encode)) + { + ROS_ERROR_STREAM("encode"); + return false; + } + if (!gst_bin_add( GST_BIN(_pipeline), _sink)) + { + ROS_ERROR_STREAM("sink"); + return false; + } + gboolean link_ok = gst_element_link_many(_source, _audioconvert, _audioresample, + _encode, _sink, NULL); + return link_ok; + } + + void onFloat(const sensor_msgs::ChannelFloat32ConstPtr &msg) + { + if (_paused) + { + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); + _paused = false; + } + + GstBuffer *buffer = gst_buffer_new_and_alloc(msg->values.size() * 4); + int num_bytes = gst_buffer_fill(buffer, 0, &msg->values[0], msg->values.size() * 4); + GstFlowReturn ret; + g_signal_emit_by_name(_source, "push-buffer", buffer, &ret); + ROS_DEBUG_STREAM("emitted push " << num_bytes << " " << ret); + } + + static GstFlowReturn onNewBuffer (GstAppSink *appsink, gpointer userData) + { + ROS_DEBUG_STREAM("new buffer"); + RosFloatToGst *server = reinterpret_cast(userData); + GstMapInfo map; + + GstSample *sample; + g_signal_emit_by_name(appsink, "pull-sample", &sample); + + GstBuffer *buffer = gst_sample_get_buffer(sample); + + if (buffer == NULL) + { + ROS_WARN_STREAM("buffer is null"); + return GST_FLOW_ERROR; + } + audio_common_msgs::AudioData msg; + gst_buffer_map(buffer, &map, GST_MAP_READ); + msg.data.resize( map.size ); + + memcpy( &msg.data[0], map.data, map.size ); + + server->publish(msg); + + return GST_FLOW_OK; + } + + void publish( const audio_common_msgs::AudioData &msg ) + { + _pub.publish(msg); + } + + static gboolean onMessage (GstBus *bus, GstMessage *message, gpointer userData) + { + RosFloatToGst *server = reinterpret_cast(userData); + GError *err; + gchar *debug; + + gst_message_parse_error(message, &err, &debug); + ROS_ERROR_STREAM("gstreamer: " << err->message); + g_error_free(err); + g_free(debug); + g_main_loop_quit(server->_loop); + server->exitOnMainThread(1); + return FALSE; + } + + static void cb_need_data (GstElement *appsrc, + guint unused_size, + gpointer user_data) + { + ROS_DEBUG_STREAM("need-data signal emitted! Pausing the pipeline"); + RosFloatToGst *client = reinterpret_cast(user_data); + gst_element_set_state(GST_ELEMENT(client->_pipeline), GST_STATE_PAUSED); + client->_paused = true; + } + + private: + ros::NodeHandle _nh; + ros::Publisher _pub; + ros::Subscriber _sub; + + boost::thread _gst_thread; + + GstElement* _pipeline; + GstElement* _source; + GstElement* _audioconvert; + GstElement* _audioresample; + GstElement* _encode; + GstElement* _sink; + + GstBus *_bus; + int _bitrate, _channels, _depth, _sample_rate; + GMainLoop *_loop; + std::string _format; + + bool _paused; + }; +} + +int main (int argc, char **argv) +{ + ros::init(argc, argv, "audio_capture"); + gst_init(&argc, &argv); + + audio_transport::RosFloatToGst server; + ros::spin(); +}