Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions audio_capture/launch/capture.launch
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
<arg name="sample_format" default="S16LE"/>
<arg name="ns" default="audio"/>
<arg name="audio_topic" default="audio"/>
<arg name="capture_volume" default="50"/>


<group ns="$(arg ns)">
<node name="audio_capture" pkg="audio_capture" type="audio_capture" output="screen">
Expand All @@ -24,6 +26,7 @@
<param name="depth" value="$(arg depth)"/>
<param name="sample_rate" value="$(arg sample_rate)"/>
<param name="sample_format" value="$(arg sample_format)"/>
<param name="capture_volume" value="$(arg capture_volume)"/>
</node>
</group>

Expand Down
66 changes: 58 additions & 8 deletions audio_capture/src/audio_capture.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ namespace audio_transport
RosGstCapture()
{
_bitrate = 192;
_capture_volume = 50; // Default volume 50%

std::string dst_type;

Expand All @@ -31,6 +32,11 @@ namespace audio_transport
ros::param::param<int>("~channels", _channels, 1);
ros::param::param<int>("~depth", _depth, 16);
ros::param::param<int>("~sample_rate", _sample_rate, 16000);

// The volume level for audio capture (0-100%)
ros::param::param<int>("~capture_volume", _capture_volume, 50);
// Ensure volume is within valid range
_capture_volume = std::max(0, std::min(_capture_volume, 100));

// The destination of the audio
ros::param::param<std::string>("~dst", dst_type, "appsink");
Expand Down Expand Up @@ -83,6 +89,18 @@ namespace audio_transport
// ghcar *gst_device = device.c_str();
g_object_set(G_OBJECT(_source), "device", device.c_str(), NULL);
}

// Create volume control element
_volume = gst_element_factory_make("volume", "volume");
if (!_volume) {
ROS_ERROR_STREAM("Failed to create volume element");
exitOnMainThread(1);
}

// Set volume level (0-100% mapped to 0.0-1.0)
double volume_value = static_cast<double>(_capture_volume) / 100.0;
g_object_set(G_OBJECT(_volume), "volume", volume_value, NULL);
ROS_INFO("Setting capture volume to %d%% (%.2f)", _capture_volume, volume_value);

GstCaps *caps;
caps = gst_caps_new_simple("audio/x-raw",
Expand Down Expand Up @@ -114,18 +132,21 @@ namespace audio_transport
g_object_set( G_OBJECT(_encode), "target", 1, NULL);
g_object_set( G_OBJECT(_encode), "bitrate", _bitrate, NULL);

gst_bin_add_many( GST_BIN(_pipeline), _source, _filter, _convert, _encode, _sink, NULL);
link_ok = gst_element_link_many(_source, _filter, _convert, _encode, _sink, NULL);
// Add volume element to pipeline
gst_bin_add_many( GST_BIN(_pipeline), _source, _filter, _volume, _convert, _encode, _sink, NULL);
link_ok = gst_element_link_many(_source, _filter, _volume, _convert, _encode, _sink, NULL);
} else if (_format == "wave") {
if (dst_type == "appsink") {
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);
// Add volume element to pipeline
gst_bin_add_many( GST_BIN(_pipeline), _source, _volume, _sink, NULL);
link_ok = gst_element_link_many( _source, _volume, _sink, NULL);
} else {
_filter = gst_element_factory_make("wavenc", "filter");
gst_bin_add_many( GST_BIN(_pipeline), _source, _filter, _sink, NULL);
link_ok = gst_element_link_many( _source, _filter, _sink, NULL);
// Add volume element to pipeline
gst_bin_add_many( GST_BIN(_pipeline), _source, _volume, _filter, _sink, NULL);
link_ok = gst_element_link_many( _source, _volume, _filter, _sink, NULL);
}
} else {
ROS_ERROR_STREAM("format must be \"wave\" or \"mp3\"");
Expand Down Expand Up @@ -159,6 +180,9 @@ namespace audio_transport
info_msg.bitrate = _bitrate;
info_msg.coding_format = _format;
_pub_info.publish(info_msg);

// Start timer to check for parameter updates
_timer = _nh.createTimer(ros::Duration(1.0), &RosGstCapture::parameterCallback, this);
}

~RosGstCapture()
Expand All @@ -168,6 +192,30 @@ namespace audio_transport
gst_object_unref(_pipeline);
g_main_loop_unref(_loop);
}

// Check for volume parameter updates
void parameterCallback(const ros::TimerEvent& event)
{
int current_volume;
if(ros::param::get("~capture_volume", current_volume))
{
if(current_volume != _capture_volume)
{
_capture_volume = std::max(0, std::min(current_volume, 100));
updateVolume();
}
}
}

// Update volume in GStreamer pipeline
void updateVolume()
{
if(_volume) {
double volume_value = static_cast<double>(_capture_volume) / 100.0;
ROS_INFO("Updating capture volume to %d%% (%.2f)", _capture_volume, volume_value);
g_object_set(G_OBJECT(_volume), "volume", volume_value, NULL);
}
}

void exitOnMainThread(int code)
{
Expand Down Expand Up @@ -242,12 +290,14 @@ namespace audio_transport
ros::Publisher _pub;
ros::Publisher _pub_stamped;
ros::Publisher _pub_info;
ros::Subscriber _volume_sub;
ros::Timer _timer;

boost::thread _gst_thread;

GstElement *_pipeline, *_source, *_filter, *_sink, *_convert, *_encode;
GstElement *_pipeline, *_source, *_filter, *_sink, *_convert, *_encode, *_volume;
GstBus *_bus;
int _bitrate, _channels, _depth, _sample_rate;
int _bitrate, _channels, _depth, _sample_rate, _capture_volume;
GMainLoop *_loop;
std::string _format, _sample_format;
};
Expand Down
3 changes: 3 additions & 0 deletions audio_play/launch/play.launch
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
<arg name="sample_format" default="S16LE"/>
<arg name="ns" default="audio"/>
<arg name="audio_topic" default="audio" />
<arg name="play_volume" default="50"/>


<group ns="$(arg ns)">
<node name="audio_play" pkg="audio_play" type="audio_play" output="screen">
Expand All @@ -24,6 +26,7 @@
<param name="depth" value="$(arg depth)"/>
<param name="sample_rate" value="$(arg sample_rate)"/>
<param name="sample_format" value="$(arg sample_format)"/>
<param name="play_volume" value="$(arg play_volume)"/>
</node>
</group>
</launch>
62 changes: 59 additions & 3 deletions audio_play/src/audio_play.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,10 @@ namespace audio_transport
ros::param::param<int>("~depth", depth, 16);
ros::param::param<int>("~sample_rate", sample_rate, 16000);
ros::param::param<std::string>("~sample_format", sample_format, "S16LE");

// The volume level for audio playback (0-100%)
ros::param::param<int>("~play_volume", _play_volume, 50);
_play_volume = std::max(0, std::min(_play_volume, 100));

_sub = _nh.subscribe("audio", 10, &RosGstPlay::onAudio, this);

Expand Down Expand Up @@ -61,14 +65,28 @@ namespace audio_transport
_convert = gst_element_factory_make("audioconvert", "convert");
audiopad = gst_element_get_static_pad(_convert, "sink");
_resample = gst_element_factory_make("audioresample", "resample");

// Create volume control element
_volume = gst_element_factory_make("volume", "volume");
if (!_volume) {
ROS_ERROR_STREAM("Failed to create volume element");
return;
}

// Set volume level (0-100% mapped to 0.0-1.0)
double volume_value = static_cast<double>(_play_volume) / 100.0;
g_object_set(G_OBJECT(_volume), "volume", volume_value, NULL);
ROS_INFO("Setting play volume to %d%% (%.2f)", _play_volume, volume_value);

_sink = gst_element_factory_make("alsasink", "sink");
g_object_set(G_OBJECT(_sink), "sync", FALSE, NULL);
if (!device.empty()) {
g_object_set(G_OBJECT(_sink), "device", device.c_str(), NULL);
}
gst_bin_add_many( GST_BIN(_audio), _convert, _resample, _sink, NULL);
gst_element_link_many(_convert, _resample, _sink, NULL);

// Add volume element to pipeline
gst_bin_add_many( GST_BIN(_audio), _convert, _resample, _volume, _sink, NULL);
gst_element_link_many(_convert, _resample, _volume, _sink, NULL);
gst_element_add_pad(_audio, gst_ghost_pad_new("sink", audiopad));
}
else
Expand Down Expand Up @@ -126,6 +144,41 @@ namespace audio_transport
//gst_element_set_state(GST_ELEMENT(_playbin), GST_STATE_PLAYING);

_gst_thread = boost::thread( boost::bind(g_main_loop_run, _loop) );

// Start timer to check for parameter updates
_timer = _nh.createTimer(ros::Duration(1.0), &RosGstPlay::parameterCallback, this);
}

~RosGstPlay()
{
g_main_loop_quit(_loop);
gst_element_set_state(_pipeline, GST_STATE_NULL);
gst_object_unref(_pipeline);
g_main_loop_unref(_loop);
}

// Check for volume parameter updates
void parameterCallback(const ros::TimerEvent& event)
{
int current_volume;
if(ros::param::get("~play_volume", current_volume))
{
if(current_volume != _play_volume)
{
_play_volume = std::max(0, std::min(current_volume, 100));
updateVolume();
}
}
}

// Update volume in GStreamer pipeline
void updateVolume()
{
if(_volume) {
double volume_value = static_cast<double>(_play_volume) / 100.0;
ROS_INFO("Updating play volume to %d%% (%.2f)", _play_volume, volume_value);
g_object_set(G_OBJECT(_volume), "volume", volume_value, NULL);
}
}

private:
Expand Down Expand Up @@ -176,11 +229,14 @@ namespace audio_transport

ros::NodeHandle _nh;
ros::Subscriber _sub;
ros::Subscriber _volume_sub;
ros::Timer _timer;
boost::thread _gst_thread;

GstElement *_pipeline, *_source, *_sink, *_decoder, *_convert, *_audio, *_resample, *_filter;
GstElement *_pipeline, *_source, *_sink, *_decoder, *_convert, *_audio, *_resample, *_filter, *_volume;
GstElement *_playbin;
GMainLoop *_loop;
int _play_volume; // Volume control (0-100%)
};
}

Expand Down