before merge

CMakeLists.txt

@@ -22,9 +22,11 @@ add_service_files(
 generate_messages(DEPENDENCIES std_msgs)
 catkin_package(
     CATKIN_DEPENDS message_runtime
+    INCLUDE_DIRS include
 )
 
 include_directories(
+    include
     ${catkin_INCLUDE_DIRS}
 )
 
@@ -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})

include/teleoperation/utils.hpp

@@ -0,0 +1,14 @@
+#ifndef _UTILS_HPP_
+#define _UTILS_HPP_
+
+#include <sstream>
+
+namespace {
+    inline std::string int_to_str(int i) {
+        std::ostringstream ss;
+        ss << i;
+        return ss.str();
+    }
+}
+
+#endif

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
+            else:
+                permission = False
 
     print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
         module, message, permission, STATE

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,

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)

src/speech.cpp

@@ -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,15 +40,15 @@ 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");
 
@@ -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() {
@@ -81,19 +84,17 @@ 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
@@ -103,7 +104,8 @@ public:
 
             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) {
 
@@ -111,18 +113,24 @@ public:
                 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();
-
-                // publish here
-                ROS_INFO("PUBLISH");
-
-                std_msgs::String c_msg;
-                c_msg.data = msg->words[0];
-                this->command_pub.publish(c_msg);
+                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;
+                    }
+                }
 
             }
             else if (msg->confidence_values[0] > 0.05) {
@@ -131,18 +139,16 @@ public:
                 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");
@@ -159,15 +165,14 @@ public:
     {
 
         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);