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

before merge

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-28 14:59:10 +01:00
parent f10e9d3ca7
commit bf217b6ac6
7 changed files with 92 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
src/speech.cpp
View File

@@ -12,6 +12,9 @@
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <teleoperation/InformController.h>
#include <teleoperation/utils.hpp>
using namespace std;
class Nao_control {
@@ -37,18 +40,18 @@ protected:
ros::Subscriber recog_sub;
// Publisher for recognized commands
ros::Publisher command_pub;
ros::ServiceClient ic;
int speech_id_ctr = 1;
std::vector<std::string> recognized_words;
bool commandfound = false;
bool imitating;
int speech_id_ctr;
public:
Nao_control(){
Nao_control() : imitating(false), speech_id_ctr(1) {
ROS_INFO("Constructor");
speech_pub = nh_.advertise<naoqi_bridge_msgs::SpeechWithFeedbackActionGoal>(
"/speech_action/goal", 1);
@@ -62,8 +65,8 @@ public:
recog_sub=nh_.subscribe("/word_recognized", 1,
&Nao_control::speechRecognitionCallback, this);
command_pub = nh_.advertise<std_msgs::String>("/recieved_commands",5);
ic = nh_.serviceClient<teleoperation::InformController>(
"/inform_controller",5);
}
~Nao_control() {
@@ -72,7 +75,7 @@ public:
//stopping recognition
std_srvs::Empty srv;
if (recog_stop_srv.call(srv)) {
ROS_INFO("SUCCESSFULLY STOPPED RECOGNITION");
}
@@ -81,93 +84,95 @@ public:
}
}
void speechRecognitionCallback(const naoqi_bridge_msgs::WordRecognized::ConstPtr& msg)
{
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;
std::cout << int_to_str(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
// 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);
s_msg.goal_id.id = int_to_str(this->speech_id_ctr);
this->speech_id_ctr += 1;
s_msg.goal.say = say;
this->speech_pub.publish(s_msg);
this->recognized_words.clear();
teleoperation::InformController ic_msg;
ic_msg.request.module = "speech_recognition";
if (msg->words[0] == "imitate") {
ic_msg.request.message = "imitate";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = true;
}
}
else if (msg->words[0] == "stop") {
ic_msg.request.message = "stop";
if (this->ic.call(ic_msg) && ic_msg.response.permission) {
this->imitating = false;
}
}
// 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) {
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);
s_msg.goal_id.id = int_to_str(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();
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");
if (!this->imitating) vocabulary.push_back("imitate");
else vocabulary.push_back("stop");
vocabulary.push_back("open");
vocabulary.push_back("close");
naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
msg.goal.words = vocabulary;
msg.goal_id.id = std::to_string(speech_id_ctr);
msg.goal_id.id = int_to_str(speech_id_ctr);
std::cout << msg << std::endl;
speech_id_ctr += 1;
voc_params_pub.publish(msg);
@@ -177,7 +182,7 @@ public:
void commandRecognition()
{
//recognition has to be started and ended once a valid command was found
initializeVocabulary();
std_srvs::Empty srv;
@@ -186,13 +191,13 @@ public:
}
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();