소스 검색

优化snowboy唤醒检测代码

corvin 5 년 전
부모
커밋
82788d2ae7

+ 1 - 1
catkin_ws/src/snowboy_wakeup/cfg/SnowboyReconfigure.cfg

@@ -5,7 +5,7 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add("sensitivity", double_t, 0, "Snowboy sensitivity", .5, 0, 1)
+gen.add("sensitivity", double_t, 0, "Snowboy sensitivity", .7, 0, 1)
 gen.add("audio_gain", double_t, 0, "Snowboy audio gain", 1, 0.01, 10)
 
 exit(gen.generate(PACKAGE, PACKAGE, "SnowboyReconfigure"))

+ 2 - 2
catkin_ws/src/snowboy_wakeup/launch/snowboy_wakeup.launch

@@ -11,11 +11,11 @@
         <remap from="audio" to="$(arg AUDIO_Topic)" />
     </node>
 
-    <node pkg="snowboy_wakeup" type="hotword_detector_node" name="snowboy" required="true">
+    <node pkg="snowboy_wakeup" type="hotword_detector_node" name="snowboy_wakeup" respawn="true">
         <param name="resource_filename" value="$(find snowboy_wakeup)/resources/common.res" />
         <param name="model_filename" value="$(find snowboy_wakeup)/resources/snowboy.umdl" />
 
-        <param name="sensitivity_str" value="0.5" type="str" />
+        <param name="sensitivity_str" value="0.7" type="str" />
         <param name="audio_gain" value="1.0" />
         <param name="asr_topic" value="$(arg ASR_Topic)" />
         <param name="audio_topic" value="$(arg AUDIO_Topic)" />

+ 51 - 53
catkin_ws/src/snowboy_wakeup/src/hotword_detector.cpp

@@ -4,58 +4,56 @@
 
 namespace snowboy_wakeup
 {
-
-HotwordDetector::HotwordDetector() : detector_(0)
-{
-}
-
-void HotwordDetector::initialize(const char* resource_filename, const char* model_filename)
-{
-  // Delete detector if we already had one
-  if (detector_)
-  {
-    delete detector_;
-  }
-
-  // We need to use cpp98 therefore we cannot pass std::strings
-  std::string resource_filename_cpp98(resource_filename);
-  std::string model_filename_cpp98(model_filename);
-
-  detector_ = new snowboy::SnowboyDetect(resource_filename_cpp98, model_filename_cpp98);
-}
-
-bool HotwordDetector::configure(double sensitivity, double audio_gain)
-{
-  // Return false if detector not initialized
-  if (!detector_)
-  {
-    return false;
-  }
-
-  std::stringstream sensitivity_ss; sensitivity_ss << sensitivity;
-
-  detector_->SetAudioGain(audio_gain);
-  detector_->SetSensitivity(sensitivity_ss.str());
-
-  return true;
-}
-
-HotwordDetector::~HotwordDetector()
-{
-  if (detector_)
-  {
-    delete detector_;
-  }
-}
-
-int HotwordDetector::runDetection(const int16_t* const data, const int array_length)
-{
-  if (!detector_)
-  {
-    return -3;
-  }
-  return detector_->RunDetection(data, array_length);
-}
-
+    HotwordDetector::HotwordDetector() : detector_(0)
+    {
+    }
+
+    void HotwordDetector::initialize(const char* resource_filename, const char* model_filename)
+    {
+        // Delete detector if we already had one
+        if (detector_)
+        {
+            delete detector_;
+        }
+
+        // We need to use cpp98 therefore we cannot pass std::strings
+        std::string resource_filename_cpp98(resource_filename);
+        std::string model_filename_cpp98(model_filename);
+
+        detector_ = new snowboy::SnowboyDetect(resource_filename_cpp98, model_filename_cpp98);
+    }
+
+    bool HotwordDetector::configure(double sensitivity, double audio_gain)
+    {
+        // Return false if detector not initialized
+        if (!detector_)
+        {
+            return false;
+        }
+
+        std::stringstream sensitivity_ss; sensitivity_ss << sensitivity;
+
+        detector_->SetAudioGain(audio_gain);
+        detector_->SetSensitivity(sensitivity_ss.str());
+
+        return true;
+    }
+
+    HotwordDetector::~HotwordDetector()
+    {
+        if (detector_)
+        {
+            delete detector_;
+        }
+    }
+
+    int HotwordDetector::runDetection(const int16_t* const data, const int array_length)
+    {
+        if (!detector_)
+        {
+            return -3;
+        }
+        return detector_->RunDetection(data, array_length);
+    }
 }
 

+ 134 - 173
catkin_ws/src/snowboy_wakeup/src/hotword_detector_node.cpp

@@ -1,195 +1,156 @@
 #include <ros/ros.h>
 #include <std_msgs/Int32.h>
+#include <snowboy_wakeup/SnowboyReconfigureConfig.h>
 #include <audio_common_msgs/AudioData.h>
 #include <dynamic_reconfigure/server.h>
-#include <snowboy_wakeup/SnowboyReconfigureConfig.h>
 #include <hotword_detector.h>
 
-#include <boost/filesystem.hpp>
-
 
 #define  ASR_START_FLAG  1  //detect hotword then start ASR
 
 namespace snowboy_wakeup
 {
-
-//!
-//! \brief The HotwordDetectorNode class Wraps the C++ 11 Snowboy detector in a ROS node
-//!
-class HotwordDetectorNode
-{
-public:
-  HotwordDetectorNode():
-    nh_(""),
-    nh_p_("~")
-  {
-  }
-
-  bool initialize()
-  {
-    std::string resource_filename;
-    if (!nh_p_.getParam("resource_filename", resource_filename))
-    {
-      ROS_ERROR("Mandatory parameter 'resource_filename' not present on the parameter server");
-      return false;
-    }
-
-    if ( !boost::filesystem::exists( resource_filename ) )
+    //!
+    //! \brief The HotwordDetectorNode class Wraps the C++ 11 Snowboy detector in a ROS node
+    //!
+    class HotwordDetectorNode
     {
-      ROS_ERROR("Resource '%s' does not exist", resource_filename.c_str());
-      return false;
-    }
-
-    std::string resource_extension = boost::filesystem::extension(resource_filename);
-    if ( resource_extension != ".res" )
-    {
-      ROS_ERROR("'%s' not a valid Snowboy resource extension ('.res').", resource_filename.c_str());
-      return false;
-    }
-
-    std::string model_filename;
-    if (!nh_p_.getParam("model_filename", model_filename))
-    {
-      ROS_ERROR("Mandatory parameter 'model_filename' not present on the parameter server");
-      return false;
-    }
+        public:
+            HotwordDetectorNode():nh_(""),nh_p_("~")
+            {}
+
+            bool initialize()
+            {
+                std::string resource_filename;
+                if (!nh_p_.getParam("resource_filename", resource_filename))
+                {
+                    ROS_ERROR("Mandatory parameter 'common.res' not present on the parameter server");
+                    return false;
+                }
+
+                std::string model_filename;
+                if (!nh_p_.getParam("model_filename", model_filename))
+                {
+                    ROS_ERROR("Mandatory parameter 'model_filename' not present on the parameter server");
+                    return false;
+                }
+
+                std::string asr_topic;
+                if (!nh_p_.getParam("asr_topic", asr_topic))
+                {
+                    ROS_ERROR("Mandatory parameter 'asr_topic' not present on the parameter server");
+                    return false;
+                }
+
+                std::string audio_topic;
+                if (!nh_p_.getParam("audio_topic", audio_topic))
+                {
+                    ROS_ERROR("Mandatory parameter 'audio_topic' not present on the parameter server");
+                    return false;
+                }
+
+                audio_sub_ = nh_.subscribe(audio_topic, 1000, &HotwordDetectorNode::audioCallback, this);
+                hotword_pub_ = nh_.advertise<std_msgs::Int32>(asr_topic, 1);
+
+                detector_.initialize(resource_filename.c_str(), model_filename.c_str());
+                dynamic_reconfigure_server_.setCallback(boost::bind(&HotwordDetectorNode::reconfigureCallback, this, _1, _2));
+
+                return true;
+            }
+
+        private:
+
+            //!
+            //! \brief nh_ Global nodehandle for topics
+            //!
+            ros::NodeHandle nh_;
+
+            //!
+            //! \brief nh_p_ Local nodehandle for parameters
+            //!
+            ros::NodeHandle nh_p_;
+
+            ros::Subscriber audio_sub_;
+            ros::Publisher hotword_pub_;
+
+            //!
+            //! \brief dynamic_reconfigure_server_ In order to online tune the sensitivity and audio gain
+            //!
+            dynamic_reconfigure::Server<SnowboyReconfigureConfig> dynamic_reconfigure_server_;
+
+            //!
+            //! \brief detector_ C++ 11 Wrapped Snowboy detect
+            //!
+            HotwordDetector detector_;
+
+            //!
+            //! \brief reconfigureCallback Reconfigure update for sensitiviy and audio level
+            //! \param cfg The updated config
+            //!
+            void reconfigureCallback(SnowboyReconfigureConfig cfg, uint32_t)
+            {
+                detector_.configure(cfg.sensitivity, cfg.audio_gain);
+                ROS_INFO("SnowboyROS (Re)Configured");
+            }
+
+            //!
+            //! \brief audioCallback Audio stream callback
+            //! \param msg The audo data
+            //!
+            void audioCallback(const audio_common_msgs::AudioDataConstPtr& msg)
+            {
+                if (msg->data.size() != 0)
+                {
+                    if ( msg->data.size() % 2 )
+                    {
+                        ROS_ERROR("Not an even number of bytes in this message!");
+                    }
+
+                    int16_t sample_array[msg->data.size()/2];
+                    for ( size_t i = 0; i < msg->data.size(); i+=2 )
+                    {
+                        sample_array[i/2] = ((int16_t) (msg->data[i+1]) << 8) + (int16_t) (msg->data[i]);
+                    }
+
+                    int result = detector_.runDetection( &sample_array[0], msg->data.size()/2);
+                    if (result > 0)
+                    {
+                        ROS_DEBUG("Hotword detected!");
+
+                        system("play -q --multi-threaded ~/Music/ding.wav");
+                        std_msgs::Int32 hotword_msg;
+                        hotword_msg.data = ASR_START_FLAG;
+                        hotword_pub_.publish(hotword_msg);
+                    }
+                    else if (result == -3)
+                    {
+                        ROS_ERROR("Hotword detector not initialized");
+                    }
+                    else if (result == -1)
+                    {
+                        ROS_ERROR("Snowboy error");
+                    }
+                }
+            }
+    };
 
-    std::string asr_topic;
-    if (!nh_p_.getParam("asr_topic", asr_topic))
-    {
-      ROS_ERROR("Mandatory parameter 'asr_topic' not present on the parameter server");
-      return false;
-    }
-
-    std::string audio_topic;
-    if (!nh_p_.getParam("audio_topic", audio_topic))
-    {
-      ROS_ERROR("Mandatory parameter 'audio_topic' not present on the parameter server");
-      return false;
-    }    
+}
 
-    if ( !boost::filesystem::exists( model_filename ) )
-    {
-      ROS_ERROR("Model '%s' does not exist", model_filename.c_str());
-      return false;
-    }
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "snowboy_wakeup_node");
+    snowboy_wakeup::HotwordDetectorNode ros_hotword_detector_node;
 
-    std::string model_extension = boost::filesystem::extension(model_filename);
-    if ( model_extension  != ".pmdl" && model_extension != ".umdl" )
+    if (ros_hotword_detector_node.initialize())
     {
-      ROS_ERROR("Model '%s', not a valid Snowboy model extension ('.pmdl', '.umdl').", resource_filename.c_str());
-      return false;
+        ros::spin();
     }
-
-    audio_sub_ = nh_.subscribe(audio_topic, 1000, &HotwordDetectorNode::audioCallback, this);
-    hotword_pub_ = nh_.advertise<std_msgs::Int32>(asr_topic, 1);
-
-    detector_.initialize(resource_filename.c_str(), model_filename.c_str());
-    dynamic_reconfigure_server_.setCallback(boost::bind(&HotwordDetectorNode::reconfigureCallback, this, _1, _2));
-
-    return true;
-  }
-
-private:
-
-  //!
-  //! \brief nh_ Global nodehandle for topics
-  //!
-  ros::NodeHandle nh_;
-
-  //!
-  //! \brief nh_p_ Local nodehandle for parameters
-  //!
-  ros::NodeHandle nh_p_;
-
-  //!
-  //! \brief audio_sub_ Subscriber to incoming audio feed
-  //!
-  ros::Subscriber audio_sub_;
-
-  //!
-  //! \brief hotword_pub_ hotword publisher
-  //!
-  ros::Publisher hotword_pub_;
-
-  //!
-  //! \brief dynamic_reconfigure_server_ In order to online tune the sensitivity and audio gain
-  //!
-  dynamic_reconfigure::Server<SnowboyReconfigureConfig> dynamic_reconfigure_server_;
-
-  //!
-  //! \brief detector_ C++ 11 Wrapped Snowboy detect
-  //!
-  HotwordDetector detector_;
-
-  //!
-  //! \brief reconfigureCallback Reconfigure update for sensitiviy and audio level
-  //! \param cfg The updated config
-  //!
-  void reconfigureCallback(SnowboyReconfigureConfig cfg, uint32_t /*level*/)
-  {
-    detector_.configure(cfg.sensitivity, cfg.audio_gain);
-    ROS_INFO("SnowboyROS (Re)Configured");
-  }
-
-  //!
-  //! \brief audioCallback Audio stream callback
-  //! \param msg The audo data
-  //!
-  void audioCallback(const audio_common_msgs::AudioDataConstPtr& msg)
-  {
-    if (msg->data.size() != 0)
+    else
     {
-      if ( msg->data.size() % 2 )
-      {
-        ROS_ERROR("Not an even number of bytes in this message!");
-      }
-
-      int16_t sample_array[msg->data.size()/2];
-      for ( size_t i = 0; i < msg->data.size(); i+=2 )
-      {
-        sample_array[i/2] = ((int16_t) (msg->data[i+1]) << 8) + (int16_t) (msg->data[i]);
-      }
-
-      int result = detector_.runDetection( &sample_array[0], msg->data.size()/2);
-      if (result > 0)
-      {
-        ROS_DEBUG("Hotword detected!");
-
-        system("play ~/Music/ding.wav");
-        std_msgs::Int32 hotword_msg;
-        hotword_msg.data = ASR_START_FLAG;
-        hotword_pub_.publish(hotword_msg);
-      }
-      else if (result == -3)
-      {
-        ROS_ERROR("Hotword detector not initialized");
-      }
-      else if (result == -1)
-      {
-        ROS_ERROR("Snowboy error");
-      }
+        ROS_ERROR("Failed to initialize snowboy_node");
+        return 1;
     }
-  }
-};
 
-}
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "snowboy_wakeup_node");
-  snowboy_wakeup::HotwordDetectorNode ros_hotword_detector_node;
-
-  if (ros_hotword_detector_node.initialize())
-  {
-    ros::spin();
-  }
-  else
-  {
-    ROS_ERROR("Failed to initialize snowboy_node");
-    return 1;
-  }
-
-  return 0;
+    return 0;
 }