159: WIP added files

src/lhw_qi/src/noice_cancellation.cpp

+#include "lhw_qi/noice_cancellation.hpp"
+
+#define _USE_MATH_DEFINES
+
+
+/*
+  For reasoning behind some of the design choices in this file, please see the qi wiki page
+*/
+
+namespace lhw_qi
+{
+
+  NoiceCancellation::NoiceCancellation(const rclcpp::NodeOptions & options)
+    : Node("noice_cancellation", options)
+  { 
+  
+    mic_buff_sub_ = this->create_subscription<lhw_interfaces::msg::Audio>("microphone_buffer", 1000);
+    //pub_ = this->create_publisher<lhw_interfaces::msg::Audio>("microphone_buffer", 1000);
+  }
+
+  void NoiceCancellation::initiate(void)
+  {
+    int front_mic_channel_id = 3;
+    listener_.call<qi::AnyValue>("setClientPreferences",
+			     "sMicrophone",
+			     16000,
+			     front_mic_channel_id, 
+			     0);
+    listener_.call<qi::AnyValue>("subscribe", "sMicrophone");
+
+
+    // Publishes all the data in the deque on the microphone_buffer topic periodically 
+    int seconds = 1;
+    int pub_nano_period = int(seconds * pow(10, 9));
+    pub_timer_ = rclcpp::create_timer(
+      this, 
+      this->get_clock(), 
+      rclcpp::Duration(std::chrono::nanoseconds(pub_nano_period)), 
+      std::bind(&Microphone::publish_buffer_cb, this)
+    );
+  }
+  
+  void Microphone::initiate(qi::SessionPtr session, boost::shared_ptr<Microphone> microphone_ptr)
+  {
+    session_ = session;
+
+    session_->waitForService("ALAudioDevice");
+    
+    listener_ = session_->service("ALAudioDevice");
+ 
+    session_->registerService("sMicrophone", qi::AnyObject( microphone_ptr ) );
+    mic_ = microphone_ptr;
+    
+    qi::AnyObject microphone = session_ -> service("sMicrophone");
+    
+    microphone.call<qi::AnyValue>("init");
+    
+  }
+  /*
+    Publishes all the data in the deque
+  */
+  void Microphone::publish_buffer_cb() {
+    if (buff_.empty()) {
+      //RCUTILS_LOG_INFO("Buffer empty!");
+      return;
+    }
+    lhw_interfaces::msg::Audio msg = lhw_interfaces::msg::Audio();
+    std::vector<uint8_t> data_vec(buff_.begin(), buff_.end());
+    int mean = calculate_buff_mean();
+    //RCUTILS_LOG_INFO("RMS: %d", mean);
+    // TODO: Make the publishing of the mean a separate message
+    msg.mean = mean;
+    msg.data = data_vec;
+    buff_.clear();
+    pub_->publish(msg);
+  }
+
+  // TODO: Fix mean value, currently there is no difference
+  // between silence and when someone is talking
+  int Microphone::calculate_buff_mean() {
+    // TODO: Possibly have to loop through buff and add together every 2 values (8 bit + 8 bit = 16 bit) 
+    std::vector<int16_t> data_16(buff_.begin(), buff_.end());
+    const int count = data_16.size();
+    int mean_ans = 0;
+    if (count != 0) {
+      auto abs_val = [](int val, int sum){return sum + std::abs(val);};
+      int mean = std::accumulate(data_16.begin(), data_16.end(), 0, abs_val);
+      //RCUTILS_LOG_INFO("MEAN: %d", mean);
+      mean_ans = mean / count;
+    }
+    //RCUTILS_LOG_INFO("COUNT: %d", count);
+    return mean_ans;
+  }
+
+
+  void Microphone::processRemote(
+    const int & nbOfChannels, 
+    const int & nbrOfSamplesByChannel, 
+    const qi::AnyValue & timestamp, 
+    const qi::AnyValue & buffer
+    ) {
+    // This needs to spin to make it possible for us to get the data from the is_talking callback
+    rclcpp::spin_some(to_std(mic_));
+    if (is_talking) return; // Stops pepper from hearing itself
+
+    std::pair<char*, size_t> charBuffer = buffer.unwrap().asRaw();
+    uint8_t* data = (uint8_t*)charBuffer.first;
+    buff_.insert(buff_.end(), data, data + charBuffer.second);
+  }
+}  // namespace lhw_qi
+
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+
+RCLCPP_COMPONENTS_REGISTER_NODE(lhw_qi::Microphone)

src/lhw_qi/src/standalone_noice_cancellation.cpp

+#include "lhw_qi/microphone_component.hpp"
+
+#include <rcutils/cmdline_parser.h>
+
+template<typename T>
+void do_release(typename boost::shared_ptr<T> const&, T*)
+{
+}
+
+template<typename T>
+typename std::shared_ptr<T> to_std(typename boost::shared_ptr<T> const& p)
+{
+    return
+        std::shared_ptr<T>(
+                p.get(),
+                boost::bind(& do_release<T>, p, _1));
+
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  // Parse the command line options.
+  auto ip = std::string("localhost");
+  char * cli_option = rcutils_cli_get_option(argv, argv + argc, "-i");
+  if (nullptr != cli_option) {
+    ip = std::string(cli_option);
+  }
+  auto port = std::string("9559");
+  cli_option = rcutils_cli_get_option(argv, argv + argc, "-p");
+  if (nullptr != cli_option) {
+    port = std::string(cli_option);
+  }
+
+  // Connect to naoqi
+  qi::ApplicationSession app(argc, argv, 0, "tcp://" + ip + ":" + port);
+  RCUTILS_LOG_INFO("Attempting connection to %s on port %s", ip.c_str(), port.c_str());
+  app.startSession();
+  qi::SessionPtr session = app.session();
+  RCUTILS_LOG_INFO("Session started");
+
+  auto microphone = boost::make_shared<lhw_qi::Microphone>(rclcpp::NodeOptions());
+  microphone->initiate(session, microphone);
+  app.run();
+  rclcpp::spin(to_std(microphone));
+  
+  rclcpp::shutdown();
+  return 0;
+}