Merge branch 'leds_feature' of gitlab.liu.se:liuhomewreckers/liu-home-wreckers into leds_feature

src/lhw_qi/CMakeLists.txt

@@ -55,6 +55,16 @@ ament_target_dependencies(motion_component
 rclcpp_components_register_nodes(motion_component "lhw_qi::Motion")
 set(node_plugins "${node_plugins}lhw_qi::Motion;$<TARGET_FILE:motion_component>\n")
 
+add_library(microphone_component SHARED
+  src/microphone_component.cpp)
+target_compile_definitions(microphone_component
+  PRIVATE "LHW_QI_BUILDING_DLL")
+ament_target_dependencies(microphone_component
+  "rclcpp"
+  "rclcpp_components")
+rclcpp_components_register_nodes(microphone_component "lhw_qi::Microphone")
+set(node_plugins "${node_plugins}lhw_qi::Microphone;$<TARGET_FILE:microphone_component>\n")
+
 
 add_library(animated_speech_component SHARED
   src/animated_speech_component.cpp)
@@ -128,6 +138,7 @@ add_executable(manual_composition
 target_link_libraries(manual_composition
   leds_component
   motion_component
+  microphone_component
   animated_speech_component
   speech_recognition_component
   touch_component
@@ -168,6 +179,17 @@ ament_target_dependencies(motion
   "rclcpp_components"
   "geometry_msgs"
 )
+
+add_executable(microphone src/standalone_microphone.cpp src/microphone_component.cpp)
+target_include_directories(microphone PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_link_libraries(microphone ${QI_LIBRARIES} ${Boost_LIBRARIES})
+ament_target_dependencies(microphone
+  "rclcpp"
+  "rclcpp_components"
+)
+
 add_executable(touch src/standalone_touch.cpp src/touch_component.cpp)
 target_include_directories(touch PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -210,6 +232,7 @@ ament_target_dependencies(camera
 install(TARGETS
   leds_component
   motion_component
+  microphone_component
   animated_speech_component
   speech_recognition_component
   touch_component
@@ -225,6 +248,7 @@ install(TARGETS
   animated_speech
   leds
   motion
+  microphone
   joint_states
   camera
   manual_composition

src/lhw_qi/include/lhw_qi/microphone_component.hpp

+#ifndef LHW_QI__MICROPHPNE_COMPONENT_HPP_
+#define LHW_QI__MICROPHPNE_COMPONENT_HPP_
+
+#include "lhw_qi/visibility_control.h"
+
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <qi/anyobject.hpp>
+#include <qi/applicationsession.hpp>
+
+
+
+namespace lhw_qi
+{
+
+  class Microphone : public rclcpp::Node
+  {
+  public:
+    LHW_QI_PUBLIC
+    explicit Microphone(const rclcpp::NodeOptions & options);
+    void initiate(qi::SessionPtr session);
+    
+    void processRemote(  const int & nbOfChannels, 
+              const int & nbrOfSamplesByChannel, 
+              const signed short * buffer, 
+              const qi::AnyValue & timeStamp);
+  //private:
+    
+    qi::SessionPtr session_;
+    qi::AnyObject microphone_service_;
+    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr start_listen_sub_;
+
+  };
+  
+}  // namespace lhw_qi
+
+
+
+#endif  // LHW_QI__LEDS_COMPONENT_HPP_

src/lhw_qi/src/microphone_component.cpp

+#include "lhw_qi/microphone_component.hpp"
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+#include <string>
+#include <iostream>
+
+namespace lhw_qi
+{
+
+  Microphone::Microphone(const rclcpp::NodeOptions & options)
+    : Node("microphone", options)
+  {
+
+    
+    
+
+    auto move_cb =
+      //Istället för Float64 ha en twist?
+      [this](const typename std_msgs::msg::Float64::SharedPtr msg) -> void
+      {
+        RCUTILS_LOG_INFO("Event detected!");
+        //motion_service_.call<void>("moveInit");
+        //motion_service_.call<void>("move", msg->data, 0.0, 0.0);
+      };
+
+
+    // Create subscribers
+    start_listen_sub_ = create_subscription<std_msgs::msg::Float64>("move", 10, move_cb);
+
+
+    RCUTILS_LOG_INFO("end of init");
+
+  }
+
+  QI_REGISTER_MT_OBJECT(Microphone, initiate, processRemote);
+
+  void Microphone::initiate(qi::SessionPtr session){
+    session_ = session;
+    microphone_service_ = session_->service("ALAudioDevice");
+
+    RCUTILS_LOG_INFO("init"); 
+    /*
+    fALMemoryKeys.push_back("ALSoundProcessing/leftMicEnergy");
+    fALMemoryKeys.push_back("ALSoundProcessing/rightMicEnergy");
+    fALMemoryKeys.push_back("ALSoundProcessing/frontMicEnergy");
+    fALMemoryKeys.push_back("ALSoundProcessing/rearMicEnergy");
+
+    fProxyToALMemory.insertData(fALMemoryKeys[0],0.0f);
+    fProxyToALMemory.insertData(fALMemoryKeys[1],0.0f);
+    fProxyToALMemory.insertData(fALMemoryKeys[2],0.0f);
+    fProxyToALMemory.insertData(fALMemoryKeys[3],0.0f);
+    */
+
+    // Do not call the function setClientPreferences in your constructor!
+    // setClientPreferences : can be called after its construction!
+    microphone_service_.call<void>("setClientPreferences",
+                          "microphone",                //Name of this module
+                          48000,                    //48000 Hz requested
+                          0,         //4 Channels requested
+                          1                         //Deinterleaving requested
+                          );
+
+    RCUTILS_LOG_INFO("after init");
+    //startDetection();
+    microphone_service_.call<void>("subscribe", "microphone");
+    RCUTILS_LOG_INFO("after init 2");
+    
+  }
+
+  void Microphone::processRemote(   const int & nbOfChannels, 
+                              const int & nbrOfSamplesByChannel, 
+                              const signed short * buffer, 
+                              const qi::AnyValue & timeStamp) {
+
+                RCUTILS_LOG_INFO("Listening!");
+
+              
+              }
+
+  
+}  // 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_microphone.cpp

+#include "lhw_qi/microphone_component.hpp"
+
+#include <rcutils/cmdline_parser.h>
+
+
+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 = std::make_shared<lhw_qi::Microphone>(rclcpp::NodeOptions());
+  microphone->initiate(session);
+  rclcpp::spin(microphone);
+
+  rclcpp::shutdown();
+  return 0;
+}