From 41634d566519c892e7f84ed0cc0f3bcf09f327e5 Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Wed, 15 Jul 2020 14:48:10 +0900
Subject: [PATCH] Use the name of the safe teleop input topics with args

---
 .../jsk_fetch_startup/launch/fetch_teleop.xml | 20 +++++++------
 .../jsk_pr2_move_base/safe_teleop.xml         | 16 +++++-----
 .../launch/safe_teleop.launch                 | 30 +++++++++++--------
 3 files changed, 38 insertions(+), 28 deletions(-)

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
index 74ff189f16..164376fbc8 100644
--- a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
+++ b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml
@@ -1,5 +1,4 @@
 <launch>
-  <machine name="localhost" address="localhost" />
   <!-- mainly copied from $(find fetch_bringup)/launch/include/teleop.launch.xml -->
   <arg name="enable_auto_dock" default="true" />
   <arg name="joy_device" default="/dev/ps3joy"/>
@@ -31,7 +30,17 @@
   </node>
 
   <!-- Mux cmd_vel topics -->
-  <include file="$(find jsk_robot_startup)/launch/safe_teleop.launch" />
+  <include file="$(find jsk_robot_startup)/launch/safe_teleop.launch" >
+    <arg name="odom_topic" value="/odom_combined"/>
+    <arg name="navigation_topic" value="/cmd_vel"/>
+  </include>
+  <!-- Relay topic names according to system diagram -->
+  <node name="relay_odom"
+        pkg="topic_tools" type="relay"
+        args="/odom /odom_teleop" />
+  <node name="relay_navigation_cmd_vel"
+        pkg="topic_tools" type="relay"
+        args="/cmd_vel /navigation/cmd_vel" />
   <group ns="safe_teleop_base">
     <rosparam file="$(find fetch_navigation)/config/costmap_common.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find fetch_navigation)/config/fetch/costmap_common.yaml" command="load" ns="local_costmap" />
@@ -64,13 +73,6 @@
       footprint_padding: 0.05
     </rosparam>
   </group>
-  <!-- Relay topic names for safe_teleop.launch -->
-  <node name="relay_odom"
-        pkg="topic_tools" type="relay"
-        args="/odom /odom_teleop" />
-  <node name="relay_navigation_cmd_vel"
-        pkg="topic_tools" type="relay"
-        args="/cmd_vel /navigation/cmd_vel" />
 
   <!-- Do not publish /base_controller/command if fetch is docking -->
   <node pkg="jsk_fetch_startup" name="warning" type="warning.py" respawn="true" />
diff --git a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml
index 9bb6c1341e..cd2a350caa 100644
--- a/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml
+++ b/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml
@@ -22,7 +22,16 @@
   <!-- Mux cmd_vel topics -->
   <include file="$(find jsk_robot_startup)/launch/safe_teleop.launch">
     <arg name="machine" value="c2" />
+    <arg name="odom_topic" value="/base_odometry/odom" />
+    <arg name="teleop_topic" value="/teleop/joy_vel" />
   </include>
+  <!-- Relay topic names according to system diagram -->
+  <node name="relay_odom"
+        pkg="topic_tools" type="relay"
+        args="/base_odometry/odom /odom_teleop" />
+  <node name="relay_teleop_cmd_vel"
+        pkg="topic_tools" type="relay"
+        args="/teleop/joy_vel /teleop/cmd_vel/unsafe" />
   <group ns="safe_teleop_base">
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/local_costmap_params.yaml" command="load" ns="local_costmap" />
@@ -33,13 +42,6 @@
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/local_costmap_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/safe_teleop_pr2_params.yaml" command="load" ns="local_costmap"/>
   </group>
-  <!-- Relay topic names for safe_teleop.launch -->
-  <node name="relay_odom"
-        pkg="topic_tools" type="relay"
-        args="/base_odometry/odom /odom_teleop" />
-  <node name="relay_teleop_cmd_vel"
-        pkg="topic_tools" type="relay"
-        args="/teleop/joy_vel /teleop/cmd_vel/unsafe" />
 
   <!-- check cable and filter baser_controller/command -->
   <node pkg="roseus" type="roseus" name="cable_warning" respawn="true"
diff --git a/jsk_robot_common/jsk_robot_startup/launch/safe_teleop.launch b/jsk_robot_common/jsk_robot_startup/launch/safe_teleop.launch
index 52df1fd256..a5c328a171 100644
--- a/jsk_robot_common/jsk_robot_startup/launch/safe_teleop.launch
+++ b/jsk_robot_common/jsk_robot_startup/launch/safe_teleop.launch
@@ -1,47 +1,53 @@
 <launch>
+  <machine name="localhost" address="localhost" />
+
   <arg name="machine" default="localhost" />
+  <arg name="odom_topic" default="/odom_teleop" />
+  <arg name="teleop_topic" default="/teleop/cmd_vel/unsafe" />
+  <arg name="navigation_topic" default="/navigation/cmd_vel" />
+  <arg name="input_topic" default="/input_vel/unsafe" />
 
-  <!-- /teleop/cmd_vel/unsafe -> /teleop/cmd_vel/safe -->
+  <!-- $(arg teleop_topic) -> /teleop/cmd_vel/safe -->
   <node name="safe_teleop_base" pkg="safe_teleop_base" type="safe_teleop_base"
         machine="$(arg machine)" >
-    <remap from="/odom" to="/odom_teleop"/>
-    <remap from="base_velocity" to="/teleop/cmd_vel/unsafe" />
+    <remap from="/odom" to="$(arg odom_topic)"/>
+    <remap from="base_velocity" to="$(arg teleop_topic)" />
     <remap from="~safe_vel"     to="/teleop/cmd_vel/safe" />
   </node>
 
-  <!-- unsafe mux: (/teleop/cmd_vel/unsafe & /teleop/cmd_vel/safe) -> /teleop/cmd_vel -->
+  <!-- unsafe mux: ($(arg teleop_topic) & /teleop/cmd_vel/safe) -> /teleop/cmd_vel -->
   <node name="unsafe_vel_mux" pkg="topic_tools" type="mux"
         respawn="true" machine="$(arg machine)"
-	args="/teleop/cmd_vel /teleop/cmd_vel/safe /teleop/cmd_vel/unsafe">
+	args="/teleop/cmd_vel /teleop/cmd_vel/safe $(arg teleop_topic)">
     <remap from="mux" to="unsafe_vel_mux" />
   </node>
   <!-- unsafe mux selector with PS3 R2 button -->
   <node name="unsafe_vel_selector" pkg="jsk_robot_startup" type="mux_selector.py"
 	respawn="true" machine="$(arg machine)"
-	args="/joy 'm.buttons[9]==1' /teleop/cmd_vel/unsafe /joy 'm.buttons[9]==0' /teleop/cmd_vel/safe">
+	args="/joy 'm.buttons[9]==1' $(arg teleop_topic) /joy 'm.buttons[9]==0' /teleop/cmd_vel/safe">
     <remap from="mux" to="unsafe_vel_mux" />
     <param name="default_select" value="/teleop/cmd_vel/safe" />
   </node>
 
-  <!-- /input_vel/unsafe -> /input_vel/safe -->
+  <!-- $(arg input_topic) -> /input_vel/safe -->
   <node name="safe_input_vel" pkg="safe_teleop_base" type="safe_teleop_base"
         machine="$(arg machine)" >
-    <remap from="/odom" to="/odom_teleop"/>
-    <remap from="base_velocity" to="/input_vel/unsafe" />
+    <remap from="/odom" to="$(arg odom_topic)"/>
+    <remap from="base_velocity" to="$(arg input_topic)" />
     <remap from="~safe_vel"     to="/input_vel/safe" />
   </node>
 
-  <!-- vel type mux: (/teleop/cmd_vel & /navigation/cmd_vel & /input_vel/safe) -> /base_controller/command_unchecked -->
+  <!-- vel type mux: (/teleop/cmd_vel & $(arg navigation_topic) & /input_vel/safe) -> /base_controller/command_unchecked -->
   <node pkg="jsk_robot_startup" type="mux_selector.py" name="vel_type_selector"
         respawn="true" output="screen" machine="$(arg machine)"
-	    args="/joy 'm.buttons[10]==1' /teleop/cmd_vel /navigation/cmd_vel 'True' /navigation/cmd_vel">
+	    args="/joy 'm.buttons[10]==1' /teleop/cmd_vel $(arg navigation_topic) 'True' $(arg navigation_topic)">
     <remap from="mux" to="vel_type_mux" />
     <param name="default_select" value="/input_vel/safe"/>
   </node>
   <!-- vel type mux with PS3 L1 button -->
   <node pkg="topic_tools" type="mux" name="vel_type_mux"
         respawn="true" machine="$(arg machine)"
-	    args="/base_controller/command_unchecked /input_vel/safe /navigation/cmd_vel /teleop/cmd_vel">
+	    args="/base_controller/command_unchecked /input_vel/safe $(arg navigation_topic) /teleop/cmd_vel">
     <remap from="mux" to="vel_type_mux" />
   </node>