From b6e7ab8aa47ab353ea7a4fadcb365e2be6bd1e64 Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Tue, 29 Jan 2019 12:07:17 +0100 Subject: [PATCH] everything seems to work together as expected (if not perfectly) --- include/teleoperation/utils.hpp | 2 +- launch/fulltest.launch | 18 +++++++++++------ script/controller.py | 25 ++++++++++++------------ script/imitator.py | 5 ++++- script/walker.py | 23 +++++++++++----------- src/aruco_detector.cpp | 7 ++++++- src/speech.cpp | 34 +++++++++++++++++++++++---------- 7 files changed, 71 insertions(+), 43 deletions(-) diff --git a/include/teleoperation/utils.hpp b/include/teleoperation/utils.hpp index 5a1023d..8459af5 100644 --- a/include/teleoperation/utils.hpp +++ b/include/teleoperation/utils.hpp @@ -4,7 +4,7 @@ #include namespace { - inline std::string int_to_str(int i) { + template inline std::string stuff_to_str(T i) { std::ostringstream ss; ss << i; return ss.str(); diff --git a/launch/fulltest.launch b/launch/fulltest.launch index 6d7042f..7e1d693 100644 --- a/launch/fulltest.launch +++ b/launch/fulltest.launch @@ -3,14 +3,20 @@ - - + + + + + + + diff --git a/script/controller.py b/script/controller.py index 4a51536..b202fac 100755 --- a/script/controller.py +++ b/script/controller.py @@ -23,6 +23,8 @@ def inform_controller_factory(who): def handle_request(r): module = r.module message = r.message + _state_old = STATE + permission = False global STATE if module == 'walker': @@ -30,12 +32,10 @@ def handle_request(r): if STATE in ('idle', 'walk'): STATE = 'walk' permission = True - else: - permission = False elif message == 'stop': if STATE == 'walk': STATE = 'idle' - permission = True + permission = True elif module == 'fall_detector': permission = True @@ -48,28 +48,27 @@ def handle_request(r): if message == 'imitate': if STATE == 'imitate': permission = True - else: - permission = False elif module == 'speech_recognition': - if message == 'imitate': + if message == 'recognize': + if STATE in ('idle', 'imitate'): + permission = True + elif message == 'imitate': if STATE == 'idle': STATE = 'imitate' permission = True - else: - permission = False - if message == 'stop': + elif message == 'stop': if STATE == 'imitate': STATE = 'idle' permission = True - else: - permission = False - rospy.loginfo( - 'Got request from %s to %s. Permission: %s. State is now: %s.' % ( + rospy.logdebug( + 'GOT REQUEST FROM %s TO %s.\nPERMISSION: %s.\nSTATE IS NOW: %s.' % ( module, message, permission, STATE ) ) + if _state_old != STATE: + rospy.loginfo('{} -> {}'.format(_state_old, STATE)) return InformControllerResponse(permission) diff --git a/script/imitator.py b/script/imitator.py index 47683fc..51ceb9d 100755 --- a/script/imitator.py +++ b/script/imitator.py @@ -18,6 +18,8 @@ if __name__ == '__main__': rospy.init_node('imitator') rospy.wait_for_service('inform_controller') ll = tf.TransformListener() + am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559) + am.setExpressiveListeningEnabled(False) mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) mp.wakeUp() mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'], @@ -27,7 +29,7 @@ if __name__ == '__main__': sleep(0.1) if not _inform_controller('imitate'): continue - rospy.loginfo('IMITATOR: ACTIVE') + rospy.logdebug('IMITATOR: ACTIVE') for i, eff in enumerate(['L', 'R'], 1): try: @@ -37,6 +39,7 @@ if __name__ == '__main__': rospy.Time(0) ) except Exception as e: + rospy.logwarn(e) continue sign = 1 if eff == 'L' else -1 diff --git a/script/walker.py b/script/walker.py index f9bf4db..e986477 100755 --- a/script/walker.py +++ b/script/walker.py @@ -57,12 +57,6 @@ if __name__ == '__main__': # print trans # continue - permission = _inform_controller('move') - - if not permission: - mp.stopMove() - continue - movement = [0, 0, 0] #-1 1 -1 1 for i, dr in enumerate((BK, FW, RT, LT)): @@ -74,12 +68,19 @@ if __name__ == '__main__': break if not any(movement): - rospy.loginfo('WALKER: STOP') + rospy.logdebug('WALKER: STOP') _inform_controller('stop') mp.move(0, 0, 0) - else: - rospy.loginfo('WALKER: TRANS: {}'.format(trans)) - rospy.loginfo('WALKER: MOVMT: {}'.format(movement)) - mp.moveToward(*movement) + continue + + permission = _inform_controller('move') + + if not permission: + mp.stopMove() + continue + + rospy.loginfo('WALKER: TRANS: {}'.format(trans)) + rospy.loginfo('WALKER: MOVMT: {}'.format(movement)) + mp.moveToward(*movement) mp.rest() diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp index e7a60b6..391c6e5 100644 --- a/src/aruco_detector.cpp +++ b/src/aruco_detector.cpp @@ -25,12 +25,16 @@ class ImageConverter { ImageConverter() : it_(nh_) { - // Subscrive to input video feed + ros::Rate loop_rate(0.1); + loop_rate.sleep(); + + // Subscribe to input video feed image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, &ImageConverter::imageCb, this); // Create output windows cv::namedWindow(ARUCO_WINDOW); + ROS_INFO("ARUCO: UP"); } ~ImageConverter() { @@ -115,6 +119,7 @@ class ImageConverter { int main(int argc, char** argv) { ros::init(argc, argv, "aruco_detector"); + ROS_INFO("ARUCO: STARTING"); ImageConverter ic; ros::spin(); return 0; diff --git a/src/speech.cpp b/src/speech.cpp index c2cfcc0..56e4a13 100644 --- a/src/speech.cpp +++ b/src/speech.cpp @@ -66,13 +66,12 @@ public: &Nao_control::speechRecognitionCallback, this); ic = nh_.serviceClient( - "/inform_controller",5); + "/inform_controller", false); } ~Nao_control() { - ROS_INFO("Destructor"); - //stopping recognition + ROS_INFO("SPEECH: DESTRUCT"); std_srvs::Empty srv; @@ -90,8 +89,7 @@ public: ROS_INFO("A WORD WAS RECOGNIZED"); std_srvs::Empty srv; - ROS_INFO("Confidence Value:"); - std::cout << int_to_str(msg->confidence_values[0]) << std::endl; + ROS_INFO("CONFIDENCE: %lf", msg->confidence_values[0]); for (int i = 0; i < msg->words.size(); i++) { std::cout << msg->words[i] << std::endl; @@ -113,7 +111,7 @@ public: std::string say = "Ok I understood " + msg->words[0]; naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg; - s_msg.goal_id.id = int_to_str(this->speech_id_ctr); + 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); @@ -125,6 +123,9 @@ public: 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"); @@ -132,6 +133,9 @@ public: if (this->ic.call(ic_msg) && ic_msg.response.permission) { this->imitating = false; } + else { + ROS_ERROR("SPEECH: CONTROLLER UNREACHABLE"); + } } } @@ -141,7 +145,7 @@ public: std::string say = "I did not understand. Could you repeat that please"; naoqi_bridge_msgs::SpeechWithFeedbackActionGoal s_msg; - s_msg.goal_id.id = int_to_str(this->speech_id_ctr); + 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); @@ -171,11 +175,12 @@ public: 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 = int_to_str(speech_id_ctr); - std::cout << msg << std::endl; + 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"); @@ -184,6 +189,16 @@ public: 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(); @@ -202,7 +217,6 @@ int main(int argc, char** argv) { ros::init(argc, argv, "speech"); Nao_control TermiNAOtor; - //TermiNAOtor.initializeVocabulary(); ros::Rate loop_rate(1); loop_rate.sleep(); TermiNAOtor.commandRecognition();