Mic remake (#59)

Dockerfile

@@ -25,7 +25,9 @@ RUN apt-get update && apt-get -y install --no-install-recommends \
     ffmpeg \
     ros-humble-image-proc \
     git \
-    htop
+    htop \
+    libsndfile1-dev \
+    libsndfile1
 
 ENV WS=/ws
 RUN mkdir -p $WS/src
@@ -46,7 +48,7 @@ RUN echo "if [ -f ${WS}/install/setup.bash ]; then source ${WS}/install/setup.ba
 RUN echo "if [ -f ${WS}/install/setup.zsh ]; then source ${WS}/install/setup.zsh; fi" >> $HOME/.zshrc
 RUN chmod +x /ws/src/rae-ros/entrypoint.sh
 
-ENTRYPOINT [ "/ws/src/rae/entrypoint.sh" ]
+ENTRYPOINT [ "/ws/src/rae-ros/entrypoint.sh" ]
 
 RUN rm -rf /usr/share/doc
 

rae_bringup/scripts/led_test.py

@@ -3,12 +3,12 @@
 from rclpy.node import Node
 from rclpy.time import Time
 
+from rae_msgs.msg import ColorPeriod
 from std_msgs.msg import ColorRGBA
 from rae_msgs.msg import LEDControl
 from cv_bridge import CvBridge
 from geometry_msgs.msg import Twist
 
-blinking = True
 
 class CarDemoNode(Node):
 
@@ -32,36 +32,35 @@ def listener_callback(self, msg):
         # Set LEDs based on battery level
         # Define colors for LEDs
         colors = {
-            "white": ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0),
-            "yellow": ColorRGBA(r=1.0, g=1.0, b=0.0, a=1.0),
-            "red": ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0),
-            "blue": ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0)
+            "white": ColorPeriod(color = ColorRGBA(r=1.0, g=1.0, b=1.0, a=1.0), frequency =0.0),
+            "yellow": ColorPeriod(color =ColorRGBA(r=1.0, g=1.0, b=0.0, a=1.0), frequency =8.0),
+            "red": ColorPeriod(color =ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0), frequency =0.0),
+            "blue": ColorPeriod(color =ColorRGBA(r=0.0, g=0.0, b=1.0, a=1.0), frequency =0.0)
         }
 
 
 
         # Create and publish LEDControl message for each LED
         led_msg = LEDControl()
         led_msg.header.stamp = self.get_clock().now().to_msg()
-        led_msg.data = [ColorRGBA(r=0.0, g=0.0, b=0.0, a=0.0)]*40
+        led_msg.data = [ColorPeriod(color =ColorRGBA(r=0.0, g=0.0, b=0.0, a=0.0), frequency =0.0)]*40
 
         for i in range(39):
             led_msg.single_led_n = 0
-            led_msg.control_type = 2 
+            led_msg.control_type = 4
             if i < 8:
                 color = "white"
                 led_msg.data[i]=(colors[color])
-            if i >9 and i < 14 and angular_speed > 0.0 and blinking==True:
+            if i >9 and i < 14 and angular_speed > 0.0:
                 color = "yellow"
                 led_msg.data[i]=(colors[color])
             if i > 20 and i < 29 and linear_speed < 0.0:
                 color = "red"
                 led_msg.data[i]=(colors[color])
-            if i> 34 and i < 39 and angular_speed < 0.0 and blinking==True:
+            if i> 34 and i < 39 and angular_speed < 0.0:
                 color = "yellow"
                 led_msg.data[i]=(colors[color])
 
-        blinking = not blinking
         self.publisher_led.publish(led_msg)
 
 
@@ -76,6 +75,5 @@ def main(args=None):
     car_demo_node.destroy_node()
     rclpy.shutdown()
 
-
 if __name__ == '__main__':
     main()

rae_hw/CMakeLists.txt

@@ -4,12 +4,15 @@ project(rae_hw)
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
+
 set(BUILD_SHARED_LIBS ON)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
 # find dependencies
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 find_package(ALSA REQUIRED)
+find_package(std_srvs REQUIRED)
 
 find_library(GPIOD_LIBRARY NAMES libgpiodcxx.so)
 if(NOT GPIOD_LIBRARY)
@@ -20,7 +23,11 @@ endif()
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(MPG123 REQUIRED libmpg123)
 
-include_directories(include )
+# Find SNDFILE library
+find_path(SNDFILE_INCLUDE_DIR sndfile.h)
+find_library(SNDFILE_LIBRARY NAMES sndfile)
+
+include_directories(include ${GST_INCLUDE_DIRS} ${MPG123_INCLUDE_DIRS} ${SNDFILE_INCLUDE_DIR})
 
 ament_auto_add_library(
   ${PROJECT_NAME}
@@ -30,25 +37,23 @@ ament_auto_add_library(
   src/peripherals/battery.cpp
   src/peripherals/lcd.cpp
   src/peripherals/led.cpp
-  src/peripherals/mic.cpp
 )
-ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES} ALSA)
 
-target_include_directories(
+ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES} ALSA std_srvs)
+
+target_link_libraries(
   ${PROJECT_NAME}
-  ${MPG123_INCLUDE_DIRS}
-  PUBLIC
-  include
+  ${MPG123_LIBRARIES}
+  ${GPIOD_LIBRARY}
+  ${SNDFILE_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")
 
 rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::BatteryNode")
 rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::LCDNode")
 rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::LEDNode")
-rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::MicNode")
 rclcpp_components_register_nodes(${PROJECT_NAME} "${PROJECT_NAME}::SpeakersNode")
 
 pluginlib_export_plugin_description_file(
@@ -87,7 +92,7 @@ ament_target_dependencies(test_speed rclcpp geometry_msgs)
 
 install(TARGETS
 
-  test_motors test_encoders test_max_speed test_speed mic_node speakers_node
+  test_motors test_encoders test_max_speed test_speed mic_node speakers_node 
 
   DESTINATION lib/${PROJECT_NAME})
 
@@ -96,8 +101,6 @@ install(
   DESTINATION include
 )
 
-
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -135,5 +138,4 @@ install(
     DESTINATION include
 )
 
-ament_package()
-
+ament_package()

rae_hw/include/rae_hw/peripherals/led.hpp

@@ -7,15 +7,20 @@
 #include <getopt.h>
 #include <fcntl.h>
 #include <time.h>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
 #include <sys/ioctl.h>
 #include <linux/ioctl.h>
 #include <sys/stat.h>
 #include <map>
+#include <cmath>
 #include <linux/types.h>
 #include "rae_hw/peripherals/spidev.h"
 #include "rclcpp/rclcpp.hpp"
 #include "rae_msgs/msg/led_control.hpp"
 
+
 #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
 #define WS2812B_NUM_LEDS 39
 #define WS2812B_RESET_PULSE 60
@@ -32,18 +37,31 @@ namespace rae_hw
     private:
         std::map<uint16_t, uint16_t> logicalToPhysicalMapping;
         void transmitSPI();
-        void fillBuffer(uint8_t color);
+        void fillBuffer(uint8_t color, float intensity = 1);
+        void LED_control ();
         uint8_t convertColor(float num);
-        void setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b);
-        void setAllPixels(uint8_t r, uint8_t g, uint8_t b);
-        void topic_callback(const rae_msgs::msg::LEDControl &msg);
+        void setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, float frequency = 0.0);
+        void setAllPixels(uint8_t r, uint8_t g, uint8_t b, float frequency = 0.0);
+        void spinner(uint8_t r, uint8_t g, uint8_t b,  uint8_t size = 5 , uint8_t blades = 1, float frequency = 0.0);
+        void fan(uint8_t r, uint8_t g, uint8_t b,  bool opening, uint8_t blades = 1, float frequency = 0.0);
+        void topic_callback(const rae_msgs::msg::LEDControl::SharedPtr msg); 
+
         rclcpp::Subscription<rae_msgs::msg::LEDControl>::SharedPtr subscription_;
         uint8_t *ptr;
         uint32_t mode;
         // static uint32_t speed = 1316134912;
         uint32_t speed = 3200000;
         uint8_t bits = 8;
+        uint32_t frame = 0;
         int fd = 0;
         uint8_t ws2812b_buffer[WS2812B_BUFFER_SIZE];
+        std::mutex mutex_;
+        std::condition_variable conditionVariable_;
+        std::thread led_control_thread_;
+        rae_msgs::msg::LEDControl::SharedPtr currentData_;
+        using SharedMsgPtr = rae_msgs::msg::LEDControl::SharedPtr;
+        rclcpp::TimerBase::SharedPtr timer_;
+        void timer_callback();
+
     };
 }

rae_hw/include/rae_hw/peripherals/mic.hpp

@@ -1,8 +1,16 @@
+// mic_node.hpp
+
 #include <vector>
 #include "rclcpp/rclcpp.hpp"
-#include "audio_msgs/msg/audio.hpp"
+#include "rae_msgs/msg/rae_audio.hpp"
 #include "alsa/asoundlib.h"
-namespace rae_hw{
+#include <rclcpp_action/rclcpp_action.hpp>
+#include "rae_msgs/action/recording.hpp"
+#include "std_srvs/srv/trigger.hpp"
+#include <sndfile.h>
+#include <rae_msgs/srv/record_audio.hpp>
+
+namespace rae_hw {
 
 class MicNode : public rclcpp::Node {
 public:
@@ -11,13 +19,21 @@ class MicNode : public rclcpp::Node {
 
 private:
     void configure_microphone();
-
     void timer_callback();
-
+    void saveToWav(const std::vector<int32_t> &buffer, snd_pcm_uframes_t frames);
+    rclcpp_action::Server<rae_msgs::action::Recording>::SharedPtr action_server_;
     rclcpp::TimerBase::SharedPtr timer_;
-    rclcpp::Publisher<audio_msgs::msg::Audio>::SharedPtr publisher_;
+    rclcpp::Publisher<rae_msgs::msg::RAEAudio>::SharedPtr publisher_;
     uint64_t seq_num_ = 0;
     snd_pcm_t *handle_;
+    bool recording_;
+    std::string wav_filename_;
+    rclcpp::Service<rae_msgs::srv::RecordAudio>::SharedPtr start_service_;
+    rclcpp::TimerBase::SharedPtr stop_timer_;
+    void startRecording(const std::shared_ptr<rae_msgs::srv::RecordAudio::Request> request,
+                        const std::shared_ptr<rae_msgs::srv::RecordAudio::Response> response);
+    void stopRecording();
+
 };
 
-}
+} // namespace rae_hw

rae_hw/launch/peripherals.launch.py

@@ -41,14 +41,15 @@ def launch_setup(context, *args, **kwargs):
                     package='rae_hw',
                     plugin='rae_hw::LEDNode',
                 )
+
             ]),
         Node(
             package='rae_hw',
-            executable='mic_node'
+            executable='speakers_node'
         ),
         Node(
             package='rae_hw',
-            executable='speakers_node'
+            executable='mic_node'
         ),
     ]
 

rae_hw/package.xml

@@ -31,7 +31,9 @@
   <depend>teleop_twist_keyboard</depend>
 
   <depend>libasound2-dev</depend> 
-  <depend>libmpg123-dev</depend> 
+  <depend>libmpg123-dev</depend>
+  <depend>libsndfile1-dev</depend>
+
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>