cleaned up a bit

launch/fulltest.launch

@@ -3,15 +3,12 @@
 <!--Launch file for Test of teleoperating NAO project-->
 
 <launch>
+
+    <include file="$(find teleoperation)/launch/sensoric.launch"/>
+
     <arg name="autoimitate" default="--no-autoimitate"/>
     <arg name="controller" default="dumb"/>
 
-    <node name="usb_cam" pkg="usb_cam" type="usb_cam_node"/>
-    <node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
-        output="screen"/>
-    <node name="speech_server" pkg="teleoperation" type="speech_server.py"
-        output="screen"/>
-
     <node name="masterloop" pkg="teleoperation" type="masterloop.py"
         output="screen" args="$(arg autoimitate)"/>
     <node name="hand_ler" pkg="teleoperation" type="hand_ler.py"/>

launch/sensoric.launch

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+
+<!--Launch file for Test of teleoperating NAO project-->
+
+<launch>
+    <node name="usb_cam" pkg="usb_cam" type="usb_cam_node"/>
+    <node name="aruco_detector" pkg="teleoperation" type="aruco_detector"
+        output="screen"/>
+    <node name="speech_server" pkg="teleoperation" type="speech_server.py"
+        output="screen"/>
+</launch>

launch/with_gui.launch

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+
+<!--Launch file for Test of teleoperating NAO project-->
+
+<launch>
+
+    <include file="$(find teleoperation)/launch/fulltest.launch"
+        pass_all_args="true"/>
+
+    <node name="human" pkg="teleoperation" type="rviz_human"/>
+    <node name="rviz_nao" pkg="rviz" type="rviz" args =
+        " -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
+    <node name="rviz_human" pkg="rviz" type="rviz"
+        args=" -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
+    <node name="nao_telemetry" pkg="rqt_gui" type="rqt_gui"/>
+
+</launch>

script/masterloop.py

@@ -33,7 +33,6 @@ AI = False  # autoimitate
 state = 'dead'  # Also walk, imitate, fallen, idle
 hand_state = 'closed'
 speech_in_progress = False
-
 mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
 
 
@@ -118,7 +117,7 @@ def handle_request(r):
         if message in ('falling', 'fallen'):
             state = 'fallen'
         elif message == 'recovered':
-            state = 'idle'
+            state = 'dead'
 
     elif module == 'imitator':
         if message == 'imitate':
@@ -161,6 +160,8 @@ if __name__ == '__main__':
     am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
     am.setExpressiveListeningEnabled(False)
 
+    hands('close')
+
     ic = rospy.Service('inform_masterloop', InformMasterloop, handle_request)
 
     def _shutdown():

scripts/speech_debug.sh

@@ -1,20 +0,0 @@
-#!/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
-finalCommand=""
-for (( i = 0; i < ${#commands[@]} ; i++ )); do
-    finalCommand+="--tab -e 'bash -c \"${commands[$i]};exec bash\"' "
-done
-
-# Run the final command
-eval "gnome-terminal "$finalCommand
-
-exit 0

scripts/teleoperation.sh

@@ -1,27 +0,0 @@
-#!/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
-    'sleep 1;cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_bringup nao_full.launch'
-    # Start speech recognition server
-    'sleep 4;cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_apps speech.launch'
-    # Launch nodes in teleoperation package
-    'sleep 4;cd ~/catkin_ws;source devel/setup.bash ;roslaunch teleoperation teleoperation.launch'
-)
-
-# Build final command with all the tabs to launch
-finalCommand=""
-for (( i = 0; i < ${#commands[@]} ; i++ )); do
-    finalCommand+="--tab -e 'bash -c \"${commands[$i]};exec bash\"' "
-done
-
-# Run the final command
-eval "gnome-terminal "$finalCommand
-
-exit 0