From 072f5add99f6533e2285fb6f4dc648e68ede0cb5 Mon Sep 17 00:00:00 2001
From: Julian Foerster <julian.foerster@mavt.ethz.ch>
Date: Fri, 10 Jan 2020 10:32:32 +0100
Subject: [PATCH 1/3] Split up fetch demo launch file to match our
 multi-machine setup

---
 .../{fetch_demo.launch => manipulation.launch}   | 16 +---------------
 moma_demos/fetch_demo/launch/multi_machine.zsh   | 10 ++++++++++
 moma_demos/fetch_demo/launch/navigation.launch   | 14 ++++++++++++++
 moma_demos/fetch_demo/launch/perception.launch   |  9 +++++++++
 .../fetch_demo/launch/user_interface.launch      | 10 ++++++++++
 5 files changed, 44 insertions(+), 15 deletions(-)
 rename moma_demos/fetch_demo/launch/{fetch_demo.launch => manipulation.launch} (64%)
 create mode 100644 moma_demos/fetch_demo/launch/multi_machine.zsh
 create mode 100644 moma_demos/fetch_demo/launch/navigation.launch
 create mode 100644 moma_demos/fetch_demo/launch/perception.launch
 create mode 100644 moma_demos/fetch_demo/launch/user_interface.launch

diff --git a/moma_demos/fetch_demo/launch/fetch_demo.launch b/moma_demos/fetch_demo/launch/manipulation.launch
similarity index 64%
rename from moma_demos/fetch_demo/launch/fetch_demo.launch
rename to moma_demos/fetch_demo/launch/manipulation.launch
index d4f7b8d00..2551719f8 100644
--- a/moma_demos/fetch_demo/launch/fetch_demo.launch
+++ b/moma_demos/fetch_demo/launch/manipulation.launch
@@ -1,14 +1,8 @@
 <?xml version="1.0" ?>
 <launch>
     <arg name="launch_rviz" default="true" />
-    <arg name="map_file" default="TODO"/>
 
-    <!-- Rigeback navigation -->
-    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
-    <include file="$(find ridgeback_navigation)/launch/include/amcl.launch" />
-    <include file="$(find ridgeback_navigation)/launch/include/move_base.launch" />
-
-    <!-- Read waypoints for approach action -->
+    <!-- Read waypoints for search action -->
     <rosparam command="load" file="$(find fetch_demo)/config/search_waypoints.yaml" />
 
     <!-- Launch GPD -->
@@ -28,14 +22,6 @@
     <node pkg="fetch_demo" type="approach_node.py" name="approach_action_node" output="screen"/>
     <node pkg="fetch_demo" type="drop_move_node.py" name="drop_action_node" output="screen"/>
 
-    <!-- Launch support nodes -->
-    <node pkg="fetch_demo" type="navigation_node.py" name="navigation_action_node" output="screen"/>
-    <node pkg="fetch_demo" type="object_detection_node.py" name="object_detection_node" output="screen"/>
-
-    <!-- Launch the execution system -->
-    <node pkg="fetch_demo" type="run_sequentially.py" name="commander_node" output="screen"/>
-
     <!-- Launch RViz -->
     <node if="$(arg launch_rviz)" type="rviz" name="rviz" pkg="rviz" args="-d $(find grasp_demo)/config/rviz.rviz"/>
-
 </launch>
diff --git a/moma_demos/fetch_demo/launch/multi_machine.zsh b/moma_demos/fetch_demo/launch/multi_machine.zsh
new file mode 100644
index 000000000..e53fc1109
--- /dev/null
+++ b/moma_demos/fetch_demo/launch/multi_machine.zsh
@@ -0,0 +1,10 @@
+#!/bin/zsh
+
+ssh asl-admin@asl-ridgeback "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo navigation.launch"
+
+ssh asl-admin@asl-yumi "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo manipulation.launch"
+
+ssh asl-admin@moma-laptop "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo perception.launch"
+
+# On local user interface machine
+roslaunch fetch_demo user_interface.launch
diff --git a/moma_demos/fetch_demo/launch/navigation.launch b/moma_demos/fetch_demo/launch/navigation.launch
new file mode 100644
index 000000000..574ec1b41
--- /dev/null
+++ b/moma_demos/fetch_demo/launch/navigation.launch
@@ -0,0 +1,14 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="launch_rviz" default="true" />
+  <arg name="map_file" default="TODO"/>
+
+  <!-- Rigeback navigation -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
+  <include file="$(find ridgeback_navigation)/launch/include/amcl.launch" />
+  <!-- TODO: AMCL is a localization system. Check if we still need this when using Maplab. -->
+
+  <include file="$(find ridgeback_navigation)/launch/include/move_base.launch" />
+  <!-- TODO The parameters loaded in these launch files do not necessarily fit our map and setup in general.
+        Might need to define our own parameter files. -->
+</launch>
diff --git a/moma_demos/fetch_demo/launch/perception.launch b/moma_demos/fetch_demo/launch/perception.launch
new file mode 100644
index 000000000..79c48e022
--- /dev/null
+++ b/moma_demos/fetch_demo/launch/perception.launch
@@ -0,0 +1,9 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="launch_rviz" default="true" />
+
+  <!-- Object detection -->
+  <node pkg="fetch_demo" type="object_detection_node.py" name="object_detection_node" output="screen"/>
+
+  <!-- TODO: launch Voxblox++ here -->
+</launch>
diff --git a/moma_demos/fetch_demo/launch/user_interface.launch b/moma_demos/fetch_demo/launch/user_interface.launch
new file mode 100644
index 000000000..a17687dd7
--- /dev/null
+++ b/moma_demos/fetch_demo/launch/user_interface.launch
@@ -0,0 +1,10 @@
+<?xml version="1.0" ?>
+<launch>
+    <arg name="launch_rviz" default="true" />
+
+    <!-- Launch the execution system -->
+    <node pkg="fetch_demo" type="run_sequentially.py" name="commander_node" output="screen"/>
+
+    <!-- Launch RViz -->
+    <node if="$(arg launch_rviz)" type="rviz" name="rviz" pkg="rviz" args="-d $(find grasp_demo)/config/rviz.rviz"/>
+</launch>

From ed02f75112dd9674a211cf64848e3db5ce0e3581 Mon Sep 17 00:00:00 2001
From: Julian Foerster <julian.foerster@mavt.ethz.ch>
Date: Fri, 10 Jan 2020 10:40:56 +0100
Subject: [PATCH 2/3] Instructions in readme

---
 moma_demos/fetch_demo/README.md               | 32 +++++++++++++++++++
 .../fetch_demo/launch/multi_machine.zsh       | 10 ------
 2 files changed, 32 insertions(+), 10 deletions(-)
 create mode 100644 moma_demos/fetch_demo/README.md
 delete mode 100644 moma_demos/fetch_demo/launch/multi_machine.zsh

diff --git a/moma_demos/fetch_demo/README.md b/moma_demos/fetch_demo/README.md
new file mode 100644
index 000000000..44331d86c
--- /dev/null
+++ b/moma_demos/fetch_demo/README.md
@@ -0,0 +1,32 @@
+
+# Fetch Demo
+
+## Launching the demo on the multi-machine setup
+
+There is one launch file for each machine in the setup. They can all be launch from the user interface laptop via SSH as follows.
+
+Navigation nodes on ridgeback machine:
+
+```bash
+ssh asl-admin@asl-ridgeback "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo navigation.launch"
+```
+
+Manipulation nodes on YuMi machine:
+
+```bash
+ssh asl-admin@asl-yumi "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo manipulation.launch"
+```
+
+Perception nodes on moma-laptop:
+
+```bash
+ssh asl-admin@moma-laptop "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo perception.launch"
+```
+
+On local user interface machine:
+
+```bash
+roslaunch fetch_demo user_interface.launch
+```
+
+Each of these commands is blocking, they should be run in separate terminals.
diff --git a/moma_demos/fetch_demo/launch/multi_machine.zsh b/moma_demos/fetch_demo/launch/multi_machine.zsh
deleted file mode 100644
index e53fc1109..000000000
--- a/moma_demos/fetch_demo/launch/multi_machine.zsh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/zsh
-
-ssh asl-admin@asl-ridgeback "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo navigation.launch"
-
-ssh asl-admin@asl-yumi "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo manipulation.launch"
-
-ssh asl-admin@moma-laptop "source /home/asl-admin/catkin_ws/devel/setup.zsh && roslaunch fetch_demo perception.launch"
-
-# On local user interface machine
-roslaunch fetch_demo user_interface.launch

From a2284f714b656a438d4461dc8f1bf57c6d72295d Mon Sep 17 00:00:00 2001
From: Julian Foerster <julian.foerster@mavt.ethz.ch>
Date: Fri, 10 Jan 2020 13:42:06 +0100
Subject: [PATCH 3/3] Added single command launch file

---
 moma_demos/fetch_demo/README.md               | 10 ++++++++++
 .../fetch_demo/launch/manipulation.launch     |  5 -----
 .../fetch_demo/launch/navigation.launch       |  2 +-
 .../fetch_demo/launch/perception.launch       |  2 --
 .../fetch_demo/launch/single_command.launch   | 20 +++++++++++++++++++
 5 files changed, 31 insertions(+), 8 deletions(-)
 create mode 100644 moma_demos/fetch_demo/launch/single_command.launch

diff --git a/moma_demos/fetch_demo/README.md b/moma_demos/fetch_demo/README.md
index 44331d86c..69d00b48f 100644
--- a/moma_demos/fetch_demo/README.md
+++ b/moma_demos/fetch_demo/README.md
@@ -3,6 +3,16 @@
 
 ## Launching the demo on the multi-machine setup
 
+### As a single launch file
+
+On the machine used for the user interface, run the following command to distribute the nodes.
+
+```bash
+roslaunch fetch_demo single_command.launch
+```
+
+### Via SSH
+
 There is one launch file for each machine in the setup. They can all be launch from the user interface laptop via SSH as follows.
 
 Navigation nodes on ridgeback machine:
diff --git a/moma_demos/fetch_demo/launch/manipulation.launch b/moma_demos/fetch_demo/launch/manipulation.launch
index 2551719f8..f3a45d102 100644
--- a/moma_demos/fetch_demo/launch/manipulation.launch
+++ b/moma_demos/fetch_demo/launch/manipulation.launch
@@ -1,7 +1,5 @@
 <?xml version="1.0" ?>
 <launch>
-    <arg name="launch_rviz" default="true" />
-
     <!-- Read waypoints for search action -->
     <rosparam command="load" file="$(find fetch_demo)/config/search_waypoints.yaml" />
 
@@ -21,7 +19,4 @@
     <node pkg="fetch_demo" type="search_node.py" name="search_action_node" output="screen"/>
     <node pkg="fetch_demo" type="approach_node.py" name="approach_action_node" output="screen"/>
     <node pkg="fetch_demo" type="drop_move_node.py" name="drop_action_node" output="screen"/>
-
-    <!-- Launch RViz -->
-    <node if="$(arg launch_rviz)" type="rviz" name="rviz" pkg="rviz" args="-d $(find grasp_demo)/config/rviz.rviz"/>
 </launch>
diff --git a/moma_demos/fetch_demo/launch/navigation.launch b/moma_demos/fetch_demo/launch/navigation.launch
index 574ec1b41..e810762c5 100644
--- a/moma_demos/fetch_demo/launch/navigation.launch
+++ b/moma_demos/fetch_demo/launch/navigation.launch
@@ -1,10 +1,10 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="launch_rviz" default="true" />
   <arg name="map_file" default="TODO"/>
 
   <!-- Rigeback navigation -->
   <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
+  
   <include file="$(find ridgeback_navigation)/launch/include/amcl.launch" />
   <!-- TODO: AMCL is a localization system. Check if we still need this when using Maplab. -->
 
diff --git a/moma_demos/fetch_demo/launch/perception.launch b/moma_demos/fetch_demo/launch/perception.launch
index 79c48e022..39abed966 100644
--- a/moma_demos/fetch_demo/launch/perception.launch
+++ b/moma_demos/fetch_demo/launch/perception.launch
@@ -1,7 +1,5 @@
 <?xml version="1.0" ?>
 <launch>
-  <arg name="launch_rviz" default="true" />
-
   <!-- Object detection -->
   <node pkg="fetch_demo" type="object_detection_node.py" name="object_detection_node" output="screen"/>
 
diff --git a/moma_demos/fetch_demo/launch/single_command.launch b/moma_demos/fetch_demo/launch/single_command.launch
new file mode 100644
index 000000000..8a60d06c0
--- /dev/null
+++ b/moma_demos/fetch_demo/launch/single_command.launch
@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<launch>
+
+  <include file="$(find fetch_demo)/launch/user_interface.launch"/>
+
+  <group>
+    <machine name="maniplation_machine" address="asl-yumi" default="true"/>
+    <include file="$(find fetch_demo)/launch/manipulation.launch"/>
+  </group>
+
+  <group>
+    <machine name="perception_machine" address="moma-laptop" default="true"/>
+    <include file="$(find fetch_demo)/launch/perception.launch"/>
+  </group>
+
+  <group>
+    <machine name="navigation_machine" address="asl-ridgeback" default="true"/>
+    <include file="$(find fetch_demo)/launch/navigation.launch"/>
+  </group>
+</launch>