merging with lukas

src/speech.cpp

@@ -0,0 +1,203 @@
+#include <iostream>
+#include <fstream>
+#include <stdlib.h>
+#include <ros/ros.h>
+#include <string.h>
+
+#include <std_srvs/Empty.h>
+#include <naoqi_bridge_msgs/SpeechWithFeedbackActionGoal.h>
+#include <actionlib_msgs/GoalStatusArray.h>
+#include <naoqi_bridge_msgs/SetSpeechVocabularyActionGoal.h>
+#include <naoqi_bridge_msgs/WordRecognized.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/String.h>
+
+using namespace std;
+
+class Nao_control {
+
+protected:
+
+    // ROS node handler
+    ros::NodeHandle nh_;
+
+    // Publisher for nao speech
+    ros::Publisher speech_pub;
+
+    // Publisher for nao vocabulary parameters
+    ros::Publisher voc_params_pub;
+
+    // Client for starting speech recognition
+    ros::ServiceClient recog_start_srv;
+
+    // Client for stopping speech recognition
+    ros::ServiceClient recog_stop_srv;
+
+    // Subscriber to speech recognition
+    ros::Subscriber recog_sub;
+
+    // Publisher for recognized commands
+    ros::Publisher command_pub; 
+
+    int speech_id_ctr = 1;
+    std::vector<std::string> recognized_words;
+    bool commandfound = false;
+
+public:
+
+    Nao_control(){
+
+        ROS_INFO("Constructor");
+ 
+        speech_pub = nh_.advertise<naoqi_bridge_msgs::SpeechWithFeedbackActionGoal>(
+                "/speech_action/goal", 1);
+
+        voc_params_pub= nh_.advertise<naoqi_bridge_msgs::SetSpeechVocabularyActionGoal>(
+                "/speech_vocabulary_action/goal", 1);
+
+        recog_start_srv=nh_.serviceClient<std_srvs::Empty>("/start_recognition");
+
+        recog_stop_srv=nh_.serviceClient<std_srvs::Empty>("/stop_recognition");
+
+        recog_sub=nh_.subscribe("/word_recognized", 1,
+                &Nao_control::speechRecognitionCallback, this);
+
+        command_pub = nh_.advertise<std_msgs::String>("/recieved_commands",5);
+        
+    }
+
+    ~Nao_control() {
+
+        ROS_INFO("Destructor");
+        //stopping recognition
+
+        std_srvs::Empty srv;
+       
+        if (recog_stop_srv.call(srv)) {
+            ROS_INFO("SUCCESSFULLY STOPPED RECOGNITION");
+        }
+        else {
+            ROS_ERROR("COULDN'T STOP RECOGNITION");
+        }
+    }
+
+    void speechRecognitionCallback(const naoqi_bridge_msgs::WordRecognized::ConstPtr& msg)
+    {
+
+        ROS_INFO("A WORD WAS RECOGNIZED");
+        std_srvs::Empty srv;
+
+        ROS_INFO("Confidence Value:");
+        std::cout << std::to_string(msg->confidence_values[0]) << std::endl;
+
+        for (int i = 0; i < msg->words.size(); i++) {
+
+            std::cout << msg->words[i] << std::endl;
+
+        }
+        
+        //set pause duration
+        double f_pause = 1;
+
+        if (recog_stop_srv.call(srv) && ((msg->words.size())> 0)) {
+
+            ROS_INFO("SUCCESSFULLY STOPPED RECOGNITION");
+       
+            // Use confidence level to decide wether the recognized word should be published
+            
+            if (msg->confidence_values[0] > 0.35) {
+
+                ROS_INFO("SPEECH STARTING");
+                std::string say = "Ok I understood";
+            
+                naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
+                s_msg.goal_id.id = std::to_string(this->speech_id_ctr);
+                this->speech_id_ctr += 1;
+                s_msg.goal.say = say;
+                this->speech_pub.publish(s_msg);
+                this->recognized_words.clear();
+
+                // publish here
+                ROS_INFO("PUBLISH");
+
+                std_msgs::String c_msg;
+                c_msg.data = msg->words[0];
+                this->command_pub.publish(c_msg);
+                
+            }
+            else if(msg->confidence_values[0] > 0.05) {    
+            
+                ROS_INFO("SPEECH STARTING");
+                std::string say = "I did not understand. Could you repeat that please";
+            
+                naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
+                s_msg.goal_id.id = std::to_string(this->speech_id_ctr);
+                this->speech_id_ctr += 1;
+                s_msg.goal.say = say;
+                this->speech_pub.publish(s_msg);
+                this->recognized_words.clear();
+
+                // increase pause duration
+                f_pause = 0.4;
+            
+          }
+
+            this->recognized_words.clear();
+        }
+        else {
+            ROS_ERROR("COULDN'T STOP RECOGNITION");
+        }    
+        // pause until NAO stops talking
+        ros::Rate loop_rate(f_pause);
+        loop_rate.sleep();
+
+        // re-start recogntion
+        commandRecognition();    
+    }
+
+    void initializeVocabulary()
+    {
+        
+        std::vector<std::string> vocabulary;
+        vocabulary.push_back("Copy postion");
+        vocabulary.push_back("Stop");// Operation ");
+        //vocabulary.push_back("hello ");
+        vocabulary.push_back("Open your hands");
+        vocabulary.push_back("Close your hands");
+
+        naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
+        msg.goal.words = vocabulary;
+        msg.goal_id.id = std::to_string(speech_id_ctr);
+        std::cout << msg << std::endl;
+        speech_id_ctr += 1;
+        voc_params_pub.publish(msg);
+        ROS_INFO("VOCABULARY INITIALIZED");
+    }
+
+    void commandRecognition()
+    {
+        //recognition has to be started and ended once a valid command was found
+        
+        initializeVocabulary();
+        std_srvs::Empty srv;
+
+        if (recog_start_srv.call(srv)) {
+            ROS_INFO("SUCCESSFULLY STARTED RECOGNITION");
+        }
+        else {
+            ROS_ERROR("COULDN'T START RECOGNITION");
+         }    
+    }
+};
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "speech");
+    
+    Nao_control TermiNAOtor;
+    ros::Rate loop_rate(0.5);
+    loop_rate.sleep();
+    TermiNAOtor.commandRecognition();
+    ROS_INFO("SPIN");
+    ros::spin();
+    return 0;
+}