Sound node (#56)

Dockerfile

@@ -25,7 +25,10 @@ RUN apt-get update \
     gstreamer1.0-plugins-bad \
     gstreamer1.0-alsa \
     libasound2-dev \
-    busybox
+    busybox \
+    alsa-utils \
+    mpg123 \
+    libmpg123-dev
 
 ENV WS=/ws
 RUN mkdir -p $WS/src

rae_hw/CMakeLists.txt

@@ -16,7 +16,16 @@ if(NOT GPIOD_LIBRARY)
   message(FATAL_ERROR "gpiod library not found. Install apt install libgpiod-dev")
 endif()
 
-include_directories(include)
+# Find mpg123 package
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(MPG123 REQUIRED libmpg123)
+
+# Add GStreamer packages
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(GST REQUIRED gstreamer-1.0)
+
+include_directories(include
+                    ${GST_INCLUDE_DIRS}  )
 
 ament_auto_add_library(
   ${PROJECT_NAME}
@@ -27,16 +36,16 @@ ament_auto_add_library(
   src/peripherals/lcd.cpp
   src/peripherals/led.cpp
   src/peripherals/mic.cpp
-  src/peripherals/speakers.cpp
 )
 ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES} ALSA)
 
 target_include_directories(
   ${PROJECT_NAME}
+  ${MPG123_INCLUDE_DIRS}
   PUBLIC
   include
 )
-target_link_libraries(${PROJECT_NAME} ${GPIOD_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${MPG123_LIBRARIES} ${GPIOD_LIBRARY})
 
 # prevent pluginlib from using boost
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
@@ -92,6 +101,8 @@ install(
   DESTINATION include
 )
 
+
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

rae_hw/include/rae_hw/peripherals/speakers.hpp

@@ -1,20 +1,34 @@
+#ifndef RAE_HW_SPEAKERS_NODE_HPP_
+#define RAE_HW_SPEAKERS_NODE_HPP_
 
 #include "rclcpp/rclcpp.hpp"
 #include "audio_msgs/msg/audio.hpp"
-
+#include "std_msgs/msg/int32.hpp"
 #include <alsa/asoundlib.h>
-namespace rae_hw{
-class SpeakersNode : public rclcpp::Node
-{
+#include <mpg123.h>
+#include <rae_msgs/srv/play_audio.hpp>
+
+namespace rae_hw {
+
+class SpeakersNode : public rclcpp::Node {
 public:
   SpeakersNode(const rclcpp::NodeOptions &options);
   ~SpeakersNode();
 
 private:
-  void audio_callback(const audio_msgs::msg::Audio::SharedPtr msg);
+  void play_mp3(const char *);
+  rclcpp::Service<rae_msgs::srv::PlayAudio>::SharedPtr play_audio_service_;
 
-  rclcpp::Subscription<audio_msgs::msg::Audio>::SharedPtr subscription_;
+  void play_audio_service_callback(
+      const std::shared_ptr<rmw_request_id_t> request_header,
+      const std::shared_ptr<rae_msgs::srv::PlayAudio::Request> request,
+      const std::shared_ptr<rae_msgs::srv::PlayAudio::Response> response);
   snd_pcm_t *alsaHandle;
+
+  unsigned char *buffer;
+  mpg123_handle *mh;
 };
 
-}
+} // namespace rae_hw
+
+#endif // RAE_HW_SPEAKERS_NODE_HPP_

rae_hw/launch/peripherals.launch.py

@@ -9,8 +9,6 @@
 
 def launch_setup(context, *args, **kwargs):
     name = LaunchConfiguration('name').perform(context)
-    enable_battery_status = LaunchConfiguration(
-        'enable_battery_status', default=True)
     run_container = LaunchConfiguration('run_container', default=True)
     return [
         ComposableNodeContainer(
@@ -52,19 +50,13 @@ def launch_setup(context, *args, **kwargs):
             package='rae_hw',
             executable='speakers_node'
         ),
-        Node(
-            package='rae_bringup',
-            executable='battery_status.py',
-            condition=IfCondition(enable_battery_status)
-        ),
     ]
 
 
 def generate_launch_description():
     declared_arguments = [
         DeclareLaunchArgument('name', default_value='rae'),
         DeclareLaunchArgument('run_container', default_value='true'),
-        DeclareLaunchArgument('enable_battery_status', default_value='true'),
         DeclareLaunchArgument('default_logo_path', default_value='/usr/share/rae-logo-white.jpg'),
     ]
 

rae_hw/package.xml

@@ -28,6 +28,9 @@
   <depend>audio_msgs</depend>
   <depend>robot_localization</depend>
 
+  <depend>libasound2-dev</depend> 
+  <depend>libmpg123-dev</depend> 
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>

rae_hw/src/speakers_node.cpp

@@ -1,16 +1,100 @@
-#include "rclcpp/rclcpp.hpp"
 #include "rae_hw/peripherals/speakers.hpp"
 
-int main(int argc, char *argv[])
-{
-    rclcpp::init(argc, argv);
+namespace rae_hw {
 
-    auto node = std::make_shared<rae_hw::SpeakersNode>(rclcpp::NodeOptions());
-    rclcpp::executors::StaticSingleThreadedExecutor executor;
-    executor.add_node(node);
-    executor.spin();
+SpeakersNode::SpeakersNode(const rclcpp::NodeOptions &options)
+    : Node("speakers_node", options) {
+  // Initialize ALSA or any other audio setup code here
 
-    rclcpp::shutdown();
+  // Create Audio Service
+  play_audio_service_ = create_service<rae_msgs::srv::PlayAudio>(
+      "play_audio", std::bind(&SpeakersNode::play_audio_service_callback, this,
+                              std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
-    return 0;
-}
+
+  // Any other initialization code here
+
+  RCLCPP_INFO(this->get_logger(), "Speakers node running!");
+}
+
+SpeakersNode::~SpeakersNode() {
+  delete[] buffer;
+  mpg123_close(mh);
+  mpg123_delete(mh);
+  mpg123_exit();
+}
+
+void SpeakersNode::play_audio_service_callback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<rae_msgs::srv::PlayAudio::Request> request,
+    const std::shared_ptr<rae_msgs::srv::PlayAudio::Response> response) {
+  // Use request->mp3_file to get the MP3 file location
+  const char *mp3_file = request->mp3_file.c_str();
+
+  // Call the play_mp3 function
+  play_mp3(mp3_file);
+
+  // Respond with success (modify based on your play_mp3 result)
+  response->success = true;
+}
+
+void SpeakersNode::play_mp3(const char *mp3_file) {
+  // Initialize libmpg123
+    mpg123_init();
+    mh = mpg123_new(NULL, NULL);
+    long rate; // Set your desired sample rate here
+    int channels, encoding;
+    if (mpg123_open(mh, mp3_file) != MPG123_OK ||
+        mpg123_getformat(mh, &rate, &channels, &encoding) != MPG123_OK) {
+        RCLCPP_ERROR(this->get_logger(), "Cant read MP3 file");
+        return;
+    }
+
+    // Open ALSA device
+    if (snd_pcm_open(&alsaHandle, "default", SND_PCM_STREAM_PLAYBACK, 0) < 0) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to open ALSA playback device.");
+        return ;
+    }
+
+
+    // Set ALSA parameters
+    if (channels==1) {
+ snd_pcm_set_params(alsaHandle, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, 1,44100, 2, 50000);
+} else if (channels==2) {
+snd_pcm_set_params(alsaHandle, SND_PCM_FORMAT_S16_LE, SND_PCM_ACCESS_RW_INTERLEAVED, 1,88200, 2, 50000);} 
+
+    // Read and play MP3 file
+    size_t buffer_size = mpg123_outblock(mh)*4;
+    unsigned char *buffer = new unsigned char[buffer_size];
+    size_t err;
+
+    while (mpg123_read(mh, buffer, buffer_size, &err) == MPG123_OK) {
+        if (snd_pcm_writei(alsaHandle, buffer, buffer_size/2) < 0) {
+            std::cerr << "Error in snd_pcm_writei: " << snd_strerror(err) << std::endl;
+        }
+    }
+
+    // Cleanup
+    delete[] buffer;
+    snd_pcm_close(alsaHandle);
+    mpg123_close(mh);
+    mpg123_delete(mh);
+    mpg123_exit();
+
+    return ;
+}
+
+} // namespace rae_hw
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+
+  auto node = std::make_shared<rae_hw::SpeakersNode>(rclcpp::NodeOptions());
+  rclcpp::executors::StaticSingleThreadedExecutor executor;
+  executor.add_node(node);
+  executor.spin();
+
+  rclcpp::shutdown();
+
+  return 0;
+}

rae_msgs/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/LEDControl.msg"
+  "srv/PlayAudio.srv"
   DEPENDENCIES std_msgs
 )
 

rae_msgs/srv/PlayAudio.srv

@@ -0,0 +1,3 @@
+string mp3_file
+---
+bool success