From a962f28c0921dd28ca68942779bcbf759e0d4161 Mon Sep 17 00:00:00 2001
From: HRS_D <francois.le-breton@tum.de>
Date: Tue, 22 Jan 2019 18:43:24 +0100
Subject: [PATCH] Timing optimization in specch node, added launch file and
 script to start application

---
 launch/teleoperation.launch | 22 ++++++++++++++
 scripts/speech_debug.sh     | 20 +++++++++++++
 scripts/teleoperation.sh    | 27 +++++++++++++++++
 src/speech.cpp              | 58 ++++++++++++++-----------------------
 4 files changed, 90 insertions(+), 37 deletions(-)
 create mode 100644 launch/teleoperation.launch
 create mode 100755 scripts/speech_debug.sh
 create mode 100755 scripts/teleoperation.sh

diff --git a/launch/teleoperation.launch b/launch/teleoperation.launch
new file mode 100644
index 0000000..d37902a
--- /dev/null
+++ b/launch/teleoperation.launch
@@ -0,0 +1,22 @@
+<!--
+Launch file for Teleoperating NAO project
+
+-->
+
+
+<launch>
+    
+    <!-- roscore is automatically started if none exists-->
+    
+    <!-- aruco detector node-->
+    <node pkg="teleoperation" type="aruco_detector" name="aruco_detector_node"/>
+
+    <!-- aruco effector node-->
+    <?ignore
+    <node pkg="teleoperation" type="aruco_effector" name="aruco_detector_node"/>
+    ?>
+
+    <!-- speech recognition and response node-->
+    <node pkg="teleoperation" type="speech" name="speech_node"/>
+
+</launch>
diff --git a/scripts/speech_debug.sh b/scripts/speech_debug.sh
new file mode 100755
index 0000000..bee05f2
--- /dev/null
+++ b/scripts/speech_debug.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+
+# Script to open debug terminal for speech node
+
+# Array of commands to run in different tabs
+commands=(
+    'cd ~/catkin_ws;source devel/setup.bash ;rostopic echo /speech_vocabulary_action/goal'
+    'cd ~/catkin_ws;source devel/setup.bash ;rostopic echo /word_recognized'
+)
+
+# Build final command with all the tabs to launch
+set finalCommand=""
+for (( i = 0; i < ${#commands[@]} ; i++ )); do
+    export finalCommand+="--tab -e 'bash -c \"${commands[$i]};exec bash\"' "
+done
+
+# Run the final command
+eval "gnome-terminal "$finalCommand
+
+exit 0
diff --git a/scripts/teleoperation.sh b/scripts/teleoperation.sh
new file mode 100755
index 0000000..c5bf314
--- /dev/null
+++ b/scripts/teleoperation.sh
@@ -0,0 +1,27 @@
+#!/bin/bash
+
+# Script to start the necessary ros functions for the Teleoperating NAO oproject
+
+# Array of commands to run in different tabs
+commands=(
+
+    # Start roscore
+    #'cd ~/catkin_ws;source devel/setup.bash ;roscore'
+    # Bringup Nao
+    'cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_bringup nao_full.launch '
+    # Start speech recognition server
+    'cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_apps speech.launch'
+    # Launch nodes in teleoperation package
+    'cd ~/catkin_ws;source devel/setup.bash ;roslaunch teleoperation teleoperation.launch'
+)
+
+# Build final command with all the tabs to launch
+set finalCommand=""
+for (( i = 0; i < ${#commands[@]} ; i++ )); do
+    export finalCommand+="--tab -e 'bash -c \"${commands[$i]};exec bash\"' "
+done
+
+# Run the final command
+eval "gnome-terminal "$finalCommand
+
+exit 0
diff --git a/src/speech.cpp b/src/speech.cpp
index b0d3490..ef19b5b 100644
--- a/src/speech.cpp
+++ b/src/speech.cpp
@@ -1,36 +1,16 @@
 #include <iostream>
 #include <fstream>
-#include <iomanip>
-#include <cmath>
 #include <stdlib.h>
 #include <ros/ros.h>
-#include <sensor_msgs/image_encodings.h>
-#include "sensor_msgs/JointState.h"
-#include "sensor_msgs/Imu.h"
-#include "message_filters/subscriber.h"
 #include <string.h>
-#include <naoqi_bridge_msgs/JointAnglesWithSpeed.h>
-#include <naoqi_bridge_msgs/Bumper.h>
-#include <naoqi_bridge_msgs/HeadTouch.h>
-#include <naoqi_bridge_msgs/HandTouch.h>
-#include <naoqi_bridge_msgs/JointAnglesWithSpeedAction.h>
+
 #include <std_srvs/Empty.h>
-#include <boost/algorithm/string.hpp>
-#include <boost/date_time.hpp>
 #include <naoqi_bridge_msgs/SpeechWithFeedbackActionGoal.h>
 #include <actionlib_msgs/GoalStatusArray.h>
-#include <naoqi_bridge_msgs/BlinkActionGoal.h>
 #include <naoqi_bridge_msgs/SetSpeechVocabularyActionGoal.h>
-#include <std_msgs/ColorRGBA.h>
 #include <naoqi_bridge_msgs/WordRecognized.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Pose2D.h>
 #include <std_msgs/Bool.h>
 #include <std_msgs/String.h>
-#include "actionlib/client/simple_action_client.h"
-
-
-
 
 using namespace std;
 
@@ -63,8 +43,6 @@ protected:
     std::vector<std::string> recognized_words;
     bool commandfound = false;
 
-    bool speech_flag = false;
-
 public:
 
     Nao_control(){
@@ -88,7 +66,6 @@ public:
         
     }
 
-
     ~Nao_control() {
 
         ROS_INFO("Destructor");
@@ -104,7 +81,6 @@ public:
         }
     }
 
-
     void speechRecognitionCallback(const naoqi_bridge_msgs::WordRecognized::ConstPtr& msg)
     {
 
@@ -119,6 +95,9 @@ public:
             std::cout << msg->words[i] << std::endl;
 
         }
+        
+        //set pause duration
+        double f_pause = 1;
 
         if (recog_stop_srv.call(srv) && ((msg->words.size())> 0)) {
 
@@ -146,7 +125,7 @@ public:
                 this->command_pub.publish(c_msg);
                 
             }
-            else if(msg->confidence_values[0] > 0.3) {    
+            else if(msg->confidence_values[0] > 0.05) {    
             
                 ROS_INFO("SPEECH STARTING");
                 std::string say = "I did not understand. Could you repeat that please";
@@ -157,29 +136,34 @@ public:
                 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();
-            ros::Rate loop_rate(10000);
-            loop_rate.sleep();
-            //program stops here, better loop structure, where recognition is started again has to be added
-            commandRecognition();
         }
         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;
-        vocabulary.push_back("Start");// Operation ");
+        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 ");
+        vocabulary.push_back("Open your hands");
+        vocabulary.push_back("Close your hands");
 
         naoqi_bridge_msgs::SetSpeechVocabularyActionGoal msg;
         msg.goal.words = vocabulary;
@@ -204,16 +188,16 @@ public:
             ROS_ERROR("COULDN'T START RECOGNITION");
          }    
     }
-
 };
 
 int main(int argc, char** argv) {
     ros::init(argc, argv, "speech");
     
     Nao_control TermiNAOtor;
-    ros::Rate loop_rate(10);
+    ros::Rate loop_rate(1);
     loop_rate.sleep();
     TermiNAOtor.commandRecognition();
+    ROS_INFO("SPIN");
     ros::spin();
     return 0;
 }