From bf217b6ac6ca538d9af979b8d80ba8e90682ddc9 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Mon, 28 Jan 2019 14:59:10 +0100 Subject: [PATCH] before merge --- CMakeLists.txt | 5 +- include/teleoperation/utils.hpp | 14 +++++ launch/teleoperation.launch | 4 +- script/controller.py | 14 ++++- script/imitator.py | 11 ++-- script/walker.py | 1 + src/speech.cpp | 101 +++++++++++++++++--------------- 7 files changed, 92 insertions(+), 58 deletions(-) create mode 100644 include/teleoperation/utils.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index c432e84..d3c2f33 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,10 +22,12 @@ add_service_files( generate_messages(DEPENDENCIES std_msgs) catkin_package( CATKIN_DEPENDS message_runtime + INCLUDE_DIRS include ) include_directories( - ${catkin_INCLUDE_DIRS} + include + ${catkin_INCLUDE_DIRS} ) catkin_install_python(PROGRAMS @@ -39,3 +41,4 @@ catkin_install_python(PROGRAMS add_executable(aruco_detector src/aruco_detector.cpp) add_executable(speech src/speech.cpp) target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB}) +target_link_libraries(speech ${catkin_LIBRARIES} ${aruco_LIB}) diff --git a/include/teleoperation/utils.hpp b/include/teleoperation/utils.hpp new file mode 100644 index 0000000..5a1023d --- /dev/null +++ b/include/teleoperation/utils.hpp @@ -0,0 +1,14 @@ +#ifndef _UTILS_HPP_ +#define _UTILS_HPP_ + +#include + +namespace { + inline std::string int_to_str(int i) { + std::ostringstream ss; + ss << i; + return ss.str(); + } +} + +#endif diff --git a/launch/teleoperation.launch b/launch/teleoperation.launch index 8a879de..8d8ae54 100644 --- a/launch/teleoperation.launch +++ b/launch/teleoperation.launch @@ -7,14 +7,14 @@ Launch file for Teleoperating NAO project - + - ?> + ?> diff --git a/script/controller.py b/script/controller.py index 7f42c72..a3afb2e 100755 --- a/script/controller.py +++ b/script/controller.py @@ -51,10 +51,20 @@ def handle_request(r): permission = True else: permission = False - elif message == 'stop': + + elif module == 'speech_recognition': + if message == 'imitate': + if STATE == 'idle': + STATE = 'imitate' + permission = True + else: + permission = False + if message == 'stop': if STATE == 'imitate': STATE = 'idle' - permission = True + permission = True + else: + permission = False print 'Got request from %s to %s. Permission: %s. State is now: %s.' % ( module, message, permission, STATE diff --git a/script/imitator.py b/script/imitator.py index 28b37f3..3f918d0 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -20,11 +20,13 @@ if __name__ == '__main__': ll = tf.TransformListener() mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) mp.wakeUp() - mp.setAngles(['LElbowRoll', 'RElbowRoll'], - [radians(-50), radians(50)], 0.5) + mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'], + [radians(-70), radians(70), -radians(40), radians(40)], 0.5) while not rospy.is_shutdown(): sleep(0.1) + if not _inform_controller('imitate'): + continue for i, eff in enumerate(['L', 'R'], 1): try: @@ -36,9 +38,6 @@ if __name__ == '__main__': except Exception as e: print e continue - permission = _inform_controller('imitate') - if not permission: - continue sign = 1 if eff == 'L' else -1 roll = asin(trans[1] / sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2)) @@ -48,6 +47,7 @@ if __name__ == '__main__': # sign = 1 if roll > 0 else -1 # roll -= sign * radians(10) # print degrees(roll) + mp.setAngles([ '{}ShoulderRoll'.format(eff), '{}ShoulderPitch'.format(eff) @@ -55,6 +55,7 @@ if __name__ == '__main__': roll, pitch ], 0.3) + # targ = np.array(trans) # targ = targ / np.linalg.norm(targ) * 0.3 # mp.setPositions(eff, _FRAME_TORSO, diff --git a/script/walker.py b/script/walker.py index c2fd1b7..bd6f93b 100755 --- a/script/walker.py +++ b/script/walker.py @@ -24,6 +24,7 @@ if __name__ == '__main__': mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) mp.wakeUp() mp.moveInit() + mp.setMoveArmsEnabled(False, False) while not rospy.is_shutdown(): sleep(0.3) diff --git a/src/speech.cpp b/src/speech.cpp index 9e71c2f..77add51 100644 --- a/src/speech.cpp +++ b/src/speech.cpp @@ -12,6 +12,9 @@ #include #include +#include +#include + 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 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( "/speech_action/goal", 1); @@ -62,8 +65,8 @@ public: recog_sub=nh_.subscribe("/word_recognized", 1, &Nao_control::speechRecognitionCallback, this); - command_pub = nh_.advertise("/recieved_commands",5); - + ic = nh_.serviceClient( + "/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 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();