did some work to assemble it together

launch/fulltest.launch

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<!--Launch file for Test of teleoperating NAO project-->
+
+<launch>
+
+    <!-- speech recognition and response node-->
+    <node name="usb_cam_node" pkg="usb_cam" type="usb_cam_node"/>
+    <node name="controller" pkg="teleoperation" type="controller.py"/>
+    <node name="speech" pkg="teleoperation" type="speech"/>
+    <node name="aruco_detector" pkg="teleoperation" type="aruco_detector"/>
+    <node name="walker" pkg="teleoperation" type="walker.py"/>
+    <node name="imitator" pkg="teleoperation" type="imitator.py"/>
+    <node name="fall_detector" pkg="teleoperation" type="fall_detector.py"/>
+
+</launch>

launch/teleoperation.launch

@@ -20,12 +20,15 @@
     <node name="speech" pkg="teleoperation" type="speech"/>
 
     <!-- open rviz window showing nao -->
-    <node name="rviz_back" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
+    <node name="rviz_back" pkg="rviz" type="rviz" args =
+        " -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
 
     <!-- open rviz window showing nao from its right side -->
-    <node name="rviz_right" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/nao_right.rviz"/>
+    <node name="rviz_right" pkg="rviz" type="rviz"
+        args=" -d $(find teleoperation)/launch/rviz_config/nao_right.rviz"/>
 
     <!-- open rviz window showing human -->
-    <node name="rviz_human" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
+    <node name="rviz_human" pkg="rviz" type="rviz"
+        args=" -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
 
 </launch>

script/controller.py

@@ -46,8 +46,7 @@ def handle_request(r):
 
     elif module == 'imitator':
         if message == 'imitate':
-            if STATE in ('imitate', 'idle'):
+            if STATE == 'imitate':
-                STATE = 'imitate'
                 permission = True
             else:
                 permission = False
@@ -66,13 +65,15 @@ def handle_request(r):
             else:
                 permission = False
 
-    print 'Got request from %s to %s. Permission: %s. State is now: %s.' % (
+    rospy.loginfo(
-        module, message, permission, STATE
+        'Got request from %s to %s. Permission: %s. State is now: %s.' % (
+            module, message, permission, STATE
+        )
     )
     return InformControllerResponse(permission)
 
 
 if __name__ == '__main__':
-    rospy.init_node('controller')
+    rospy.init_node('controller', log_level=rospy.INFO)
     ic = rospy.Service('inform_controller', InformController, handle_request)
     rospy.spin()

script/imitator.py

@@ -27,6 +27,7 @@ if __name__ == '__main__':
         sleep(0.1)
         if not _inform_controller('imitate'):
             continue
+        rospy.loginfo('IMITATOR: ACTIVE')
         for i, eff in enumerate(['L',
                                  'R'], 1):
             try:
@@ -36,18 +37,14 @@ if __name__ == '__main__':
                     rospy.Time(0)
                 )
             except Exception as e:
-                print e
                 continue
+
             sign = 1 if eff == 'L' else -1
             roll = asin(trans[1] /
                         sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
             roll -= sign * radians(25)
             pitch = atan(-trans[2] / abs(trans[0]))
 
-            # sign = 1 if roll > 0 else -1
-            # roll -= sign * radians(10)
-            # print degrees(roll)
-
             mp.setAngles([
                 '{}ShoulderRoll'.format(eff),
                 '{}ShoulderPitch'.format(eff)

script/walker.py

@@ -8,15 +8,32 @@ from naoqi import ALProxy
 
 from controller import inform_controller_factory
 
-FW = -0.32
+
-BK = -0.55
+      #min   #max
-LT = -0.55
+FW =  1.65,  1.45
-RT = 0.0
+BK =  2.20,  2.40
+LT = -0.35, -0.53
+RT =  0.35,  0.53
+
+VMIN = 0.3
+VMAX = 1.0
 
 
 _inform_controller = inform_controller_factory('walker')
 
 
+def _speed(pos, interval):
+    int_dir = 1 if interval[1] > interval[0] else -1
+    if int_dir * (pos - interval[0]) < 0:
+        return 0.0
+    elif int_dir * (pos - interval[1]) > 0:
+        return 1.0
+    else:
+        return (VMAX - VMIN) * abs(pos - interval[0]) / (
+            abs(interval[1] - interval[0])
+        ) + VMIN
+
+
 if __name__ == '__main__':
     rospy.init_node('walker')
     rospy.wait_for_service('inform_controller')
@@ -30,37 +47,39 @@ if __name__ == '__main__':
         sleep(0.3)
         try:
             trans, rot = ll.lookupTransform('Aruco_0_frame',
-                                            'CameraTop_optical_frame',
+                                            'odom',
                                             rospy.Time(0))
         except Exception as e:
             mp.stopMove()
             _inform_controller('stop')
             continue
 
-        print trans, rot
+        # print trans
         # continue
 
-        if (
-            BK < trans[2] < FW and
-            LT < trans[0] < RT
-            # CW < trans[1] < CC
-        ):
-            _inform_controller('stop')
-            mp.move(0, 0, 0)
-            continue
-
         permission = _inform_controller('move')
+
         if not permission:
             mp.stopMove()
             continue
 
-        if trans[2] < BK:  # backwards
+        movement = [0, 0, 0]
-            mp.move(-1, 0, 0)
+                               #-1   1  -1   1
-        elif FW < trans[2]:  # forwards
+        for i, dr in enumerate((BK, FW, RT, LT)):
-            mp.move(1, 0, 0)
+            idx = i // 2
-        elif RT < trans[0]:  # right
+            sign = 1 if (i % 2) else -1
-            mp.move(0, -1, 0)
+            speed = _speed(trans[idx], dr)
-        elif trans[0] < LT:  # left
+            if speed:
-            mp.move(0, 1, 0)
+                movement[idx] = sign * speed
+                break
+
+        if not any(movement):
+            rospy.loginfo('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)
 
     mp.rest()

src/speech.cpp

@@ -98,7 +98,7 @@ public:
         }
 
         //set pause duration
-        double f_pause = 1;
+        double f_pause = 2;
 
         if (recog_stop_srv.call(srv) && ((msg->words.size())> 0)) {
 
@@ -110,7 +110,7 @@ public:
             if (msg->confidence_values[0] > 0.35) {
 
                 ROS_INFO("SPEECH STARTING");
-                std::string say = "Ok I understood";
+                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);
@@ -121,11 +121,13 @@ public:
                 ic_msg.request.module = "speech_recognition";
                 if (msg->words[0] == "imitate") {
                     ic_msg.request.message = "imitate";
+                    ROS_INFO("SPEECH: REQUESTING IMITATION");
                     if (this->ic.call(ic_msg) && ic_msg.response.permission) {
                         this->imitating = true;
                     }
                 }
                 else if (msg->words[0] == "stop") {
+                    ROS_INFO("SPEECH: REQUESTING STOP IMITATION");
                     ic_msg.request.message = "stop";
                     if (this->ic.call(ic_msg) && ic_msg.response.permission) {
                         this->imitating = false;