From 567f7b0274c3223e3d0a68da7fd3527a9d7e1d56 Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@tum.de>
Date: Sun, 10 Feb 2019 10:20:52 +0100
Subject: [PATCH] cleaned up a bit

---
 launch/fulltest.launch   |  9 +++------
 launch/sensoric.launch   | 11 +++++++++++
 launch/with_gui.launch   | 17 +++++++++++++++++
 script/masterloop.py     |  5 +++--
 scripts/speech_debug.sh  | 20 --------------------
 scripts/teleoperation.sh | 27 ---------------------------
 6 files changed, 34 insertions(+), 55 deletions(-)
 create mode 100644 launch/sensoric.launch
 create mode 100644 launch/with_gui.launch
 delete mode 100755 scripts/speech_debug.sh
 delete mode 100755 scripts/teleoperation.sh

diff --git a/launch/fulltest.launch b/launch/fulltest.launch
index 980f19e..7c622f8 100644
--- a/launch/fulltest.launch
+++ b/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"/>
diff --git a/launch/sensoric.launch b/launch/sensoric.launch
new file mode 100644
index 0000000..cf4f073
--- /dev/null
+++ b/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>
diff --git a/launch/with_gui.launch b/launch/with_gui.launch
new file mode 100644
index 0000000..9b92791
--- /dev/null
+++ b/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>
diff --git a/script/masterloop.py b/script/masterloop.py
index cf6e5d6..369c589 100755
--- a/script/masterloop.py
+++ b/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():
diff --git a/scripts/speech_debug.sh b/scripts/speech_debug.sh
deleted file mode 100755
index 440ea90..0000000
--- a/scripts/speech_debug.sh
+++ /dev/null
@@ -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
diff --git a/scripts/teleoperation.sh b/scripts/teleoperation.sh
deleted file mode 100755
index 83616a2..0000000
--- a/scripts/teleoperation.sh
+++ /dev/null
@@ -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