pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

more stable implementation of speech stuff

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-02 13:38:35 +01:00
parent 3217b7f841
commit 655c5418fd
7 changed files with 77 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files

226
src/speech.cpp
View File

@@ -1,226 +0,0 @@
#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>
#include <teleoperation/InformController.h>
#include <teleoperation/utils.hpp>
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::ServiceClient ic;
bool imitating;
int speech_id_ctr;
public:
Nao_control() : imitating(false), speech_id_ctr(1) {
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);
ic = nh_.serviceClient<teleoperation::InformController>(
"/inform_controller", false);
}
~Nao_control() {
ROS_INFO("SPEECH: DESTRUCT");
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: %lf", msg->confidence_values[0]);
for (int i = 0; i < msg->words.size(); i++) {
std::cout << msg->words[i] << std::endl;
}
//set pause duration
double f_pause = 2;
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 " + msg->words[0];
naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg;
s_msg.goal_id.id = stuff_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1;
s_msg.goal.say = say;
this->speech_pub.publish(s_msg);
teleoperation::InformController ic_msg;
ic_msg.request.module = "speech_recognition";
if (msg->words[0] == "imitate") {
ic_msg.request.message = "imitate";
ROS_INFO("SPEECH: REQUESTING IMITATION");
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = true;
}
else {
ROS_ERROR("SPEECH: CONTROLLER UNREACHABLE");
}
}
else if (msg->words[0] == "stop") {
ROS_INFO("SPEECH: REQUESTING STOP IMITATION");
ic_msg.request.message = "stop";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = false;
}
else {
ROS_ERROR("SPEECH: CONTROLLER UNREACHABLE");
}
}
}
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 = stuff_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1;
s_msg.goal.say = say;
this->speech_pub.publish(s_msg);
// increase pause duration
f_pause = 0.4;
}
}
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;
if (!this->imitating) vocabulary.push_back("imitate");
else vocabulary.push_back("stop");
vocabulary.push_back("open");
vocabulary.push_back("close");
// vocabulary.push_back("kill");
naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
msg.goal.words = vocabulary;
msg.goal_id.id = stuff_to_str(speech_id_ctr);
std::cout << msg.goal << 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
while (true) {
ros::Rate loop_rate(4);
loop_rate.sleep();
teleoperation::InformController ic_msg;
ic_msg.request.module = "speech_recognition";
ic_msg.request.message = "recognize";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
break;
}
}
this->initializeVocabulary();
ros::Rate loop_rate(1);
loop_rate.sleep();
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(1);
loop_rate.sleep();
TermiNAOtor.commandRecognition();
ROS_INFO("SPIN");
ros::spin();
return 0;
}