|
| 1 | +#include <stdio.h> |
| 2 | +#include <gst/gst.h> |
| 3 | +#include <gst/app/gstappsink.h> |
| 4 | +#include <boost/thread.hpp> |
| 5 | + |
| 6 | +#include <ros/ros.h> |
| 7 | + |
| 8 | +#include "audio_common_msgs/AudioData.h" |
| 9 | + |
| 10 | +namespace audio_transport |
| 11 | +{ |
| 12 | + class RosGstCapture |
| 13 | + { |
| 14 | + public: |
| 15 | + RosGstCapture() |
| 16 | + { |
| 17 | + _bitrate = 192; |
| 18 | + |
| 19 | + std::string dst_type; |
| 20 | + |
| 21 | + // Need to encoding or publish raw wave data |
| 22 | + ros::param::param<std::string>("~format", _format, "mp3"); |
| 23 | + |
| 24 | + // The bitrate at which to encode the audio |
| 25 | + ros::param::param<int>("~bitrate", _bitrate, 192); |
| 26 | + |
| 27 | + // only available for raw data |
| 28 | + ros::param::param<int>("~channels", _channels, 1); |
| 29 | + ros::param::param<int>("~depth", _depth, 16); |
| 30 | + ros::param::param<int>("~sample_rate", _sample_rate, 16000); |
| 31 | + |
| 32 | + // The destination of the audio |
| 33 | + ros::param::param<std::string>("~dst", dst_type, "appsink"); |
| 34 | + |
| 35 | + // The source of the audio |
| 36 | + //ros::param::param<std::string>("~src", source_type, "alsasrc"); |
| 37 | + |
| 38 | + _pub = _nh. advertise<audio_common_msgs::AudioData>( "audio", 10, true); |
| 39 | + |
| 40 | + _loop = g_main_loop_new(NULL, false); |
| 41 | + _pipeline = gst_pipeline_new("ros_pipeline"); |
| 42 | + _bus = gst_pipeline_get_bus(GST_PIPELINE(_pipeline)); |
| 43 | + gst_bus_add_signal_watch(_bus); |
| 44 | + g_signal_connect(_bus, "message::error", |
| 45 | + G_CALLBACK(onMessage), this); |
| 46 | + g_object_unref(_bus); |
| 47 | + |
| 48 | + // We create the sink first, just for convenience |
| 49 | + if (dst_type == "appsink") |
| 50 | + { |
| 51 | + _sink = gst_element_factory_make("appsink", "sink"); |
| 52 | + g_object_set(G_OBJECT(_sink), "emit-signals", true, NULL); |
| 53 | + g_object_set(G_OBJECT(_sink), "max-buffers", 100, NULL); |
| 54 | + g_signal_connect( G_OBJECT(_sink), "new-buffer", |
| 55 | + G_CALLBACK(onNewBuffer), this); |
| 56 | + } |
| 57 | + else |
| 58 | + { |
| 59 | + printf("file sink\n"); |
| 60 | + _sink = gst_element_factory_make("filesink", "sink"); |
| 61 | + g_object_set( G_OBJECT(_sink), "location", dst_type.c_str(), NULL); |
| 62 | + } |
| 63 | + |
| 64 | + _source = gst_element_factory_make("alsasrc", "source"); |
| 65 | + _convert = gst_element_factory_make("audioconvert", "convert"); |
| 66 | + |
| 67 | + gboolean link_ok; |
| 68 | + |
| 69 | + if (_format == "mp3"){ |
| 70 | + _encode = gst_element_factory_make("lame", "encoder"); |
| 71 | + g_object_set( G_OBJECT(_encode), "preset", 1001, NULL); |
| 72 | + g_object_set( G_OBJECT(_encode), "bitrate", _bitrate, NULL); |
| 73 | + |
| 74 | + gst_bin_add_many( GST_BIN(_pipeline), _source, _convert, _encode, _sink, NULL); |
| 75 | + link_ok = gst_element_link_many(_source, _convert, _encode, _sink, NULL); |
| 76 | + } else if (_format == "wave") { |
| 77 | + GstCaps *caps; |
| 78 | + caps = gst_caps_new_simple("audio/x-raw-int", |
| 79 | + "channels", G_TYPE_INT, _channels, |
| 80 | + "width", G_TYPE_INT, _depth, |
| 81 | + "depth", G_TYPE_INT, _depth, |
| 82 | + "rate", G_TYPE_INT, _sample_rate, |
| 83 | + "signed", G_TYPE_BOOLEAN, TRUE, |
| 84 | + NULL); |
| 85 | + |
| 86 | + g_object_set( G_OBJECT(_sink), "caps", caps, NULL); |
| 87 | + gst_caps_unref(caps); |
| 88 | + gst_bin_add_many( GST_BIN(_pipeline), _source, _sink, NULL); |
| 89 | + link_ok = gst_element_link_many( _source, _sink, NULL); |
| 90 | + } else { |
| 91 | + ROS_ERROR_STREAM("format must be \"wave\" or \"mp3\""); |
| 92 | + exitOnMainThread(1); |
| 93 | + } |
| 94 | + /*} |
| 95 | + else |
| 96 | + { |
| 97 | + _sleep_time = 10000; |
| 98 | + _source = gst_element_factory_make("filesrc", "source"); |
| 99 | + g_object_set(G_OBJECT(_source), "location", source_type.c_str(), NULL); |
| 100 | +
|
| 101 | + gst_bin_add_many( GST_BIN(_pipeline), _source, _sink, NULL); |
| 102 | + gst_element_link_many(_source, _sink, NULL); |
| 103 | + } |
| 104 | + */ |
| 105 | + |
| 106 | + if (!link_ok) { |
| 107 | + ROS_ERROR_STREAM("Unsupported media type."); |
| 108 | + exitOnMainThread(1); |
| 109 | + } |
| 110 | + |
| 111 | + gst_element_set_state(GST_ELEMENT(_pipeline), GST_STATE_PLAYING); |
| 112 | + |
| 113 | + _gst_thread = boost::thread( boost::bind(g_main_loop_run, _loop) ); |
| 114 | + } |
| 115 | + |
| 116 | + ~RosGstCapture() |
| 117 | + { |
| 118 | + g_main_loop_quit(_loop); |
| 119 | + gst_element_set_state(_pipeline, GST_STATE_NULL); |
| 120 | + gst_object_unref(_pipeline); |
| 121 | + g_main_loop_unref(_loop); |
| 122 | + } |
| 123 | + |
| 124 | + void exitOnMainThread(int code) |
| 125 | + { |
| 126 | + exit(code); |
| 127 | + } |
| 128 | + |
| 129 | + void publish( const audio_common_msgs::AudioData &msg ) |
| 130 | + { |
| 131 | + _pub.publish(msg); |
| 132 | + } |
| 133 | + |
| 134 | + static GstFlowReturn onNewBuffer (GstAppSink *appsink, gpointer userData) |
| 135 | + { |
| 136 | + RosGstCapture *server = reinterpret_cast<RosGstCapture*>(userData); |
| 137 | + |
| 138 | + GstBuffer *buffer; |
| 139 | + g_signal_emit_by_name(appsink, "pull-buffer", &buffer); |
| 140 | + |
| 141 | + audio_common_msgs::AudioData msg; |
| 142 | + msg.data.resize( buffer->size ); |
| 143 | + memcpy( &msg.data[0], buffer->data, buffer->size); |
| 144 | + |
| 145 | + server->publish(msg); |
| 146 | + |
| 147 | + return GST_FLOW_OK; |
| 148 | + } |
| 149 | + |
| 150 | + static gboolean onMessage (GstBus *bus, GstMessage *message, gpointer userData) |
| 151 | + { |
| 152 | + RosGstCapture *server = reinterpret_cast<RosGstCapture*>(userData); |
| 153 | + GError *err; |
| 154 | + gchar *debug; |
| 155 | + |
| 156 | + gst_message_parse_error(message, &err, &debug); |
| 157 | + ROS_ERROR_STREAM("gstreamer: " << err->message); |
| 158 | + g_error_free(err); |
| 159 | + g_free(debug); |
| 160 | + g_main_loop_quit(server->_loop); |
| 161 | + server->exitOnMainThread(1); |
| 162 | + return FALSE; |
| 163 | + } |
| 164 | + |
| 165 | + private: |
| 166 | + ros::NodeHandle _nh; |
| 167 | + ros::Publisher _pub; |
| 168 | + |
| 169 | + boost::thread _gst_thread; |
| 170 | + |
| 171 | + GstElement *_pipeline, *_source, *_sink, *_convert, *_encode; |
| 172 | + GstBus *_bus; |
| 173 | + int _bitrate, _channels, _depth, _sample_rate; |
| 174 | + GMainLoop *_loop; |
| 175 | + std::string _format; |
| 176 | + }; |
| 177 | +} |
| 178 | + |
| 179 | +int main (int argc, char **argv) |
| 180 | +{ |
| 181 | + ros::init(argc, argv, "audio_capture"); |
| 182 | + gst_init(&argc, &argv); |
| 183 | + |
| 184 | + audio_transport::RosGstCapture server; |
| 185 | + ros::spin(); |
| 186 | +} |
0 commit comments