From 29890499d31243c00fd819b8acf2a9bab55ed1cd Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Tue, 7 Sep 2021 03:25:11 -0700
Subject: [PATCH] Remove rtabmap launch files from Stretch Navigation

---
 stretch_navigation/launch/rtab.launch         | 61 -----------------
 stretch_navigation/launch/rtab/robot.launch   | 36 ----------
 stretch_navigation/launch/rtab/rtab.launch    | 57 ----------------
 stretch_navigation/launch/rtab_generic.launch | 65 -------------------
 stretch_navigation/launch/rtab_robot.launch   | 51 ---------------
 stretch_navigation/launch/rviz_rtab.launch    |  3 -
 stretch_navigation/launch/teleop.launch       |  7 --
 .../launch/rtab_gazebo.launch                 |  0
 .../launch/rtabmapviz.launch                  |  0
 9 files changed, 280 deletions(-)
 delete mode 100644 stretch_navigation/launch/rtab.launch
 delete mode 100644 stretch_navigation/launch/rtab/robot.launch
 delete mode 100644 stretch_navigation/launch/rtab/rtab.launch
 delete mode 100644 stretch_navigation/launch/rtab_generic.launch
 delete mode 100644 stretch_navigation/launch/rtab_robot.launch
 delete mode 100644 stretch_navigation/launch/rviz_rtab.launch
 delete mode 100644 stretch_navigation/launch/teleop.launch
 rename {stretch_navigation => stretch_rtab}/launch/rtab_gazebo.launch (100%)
 rename {stretch_navigation => stretch_rtab}/launch/rtabmapviz.launch (100%)

diff --git a/stretch_navigation/launch/rtab.launch b/stretch_navigation/launch/rtab.launch
deleted file mode 100644
index f2876ea..0000000
--- a/stretch_navigation/launch/rtab.launch
+++ /dev/null
@@ -1,61 +0,0 @@
-<launch>
-  
-  <arg name="database_path"     default="rtabmap.db"/>
-  <arg name="args"              default=""/>
-  
-  <arg name="wait_for_transform"  default="0.2"/> 
-
-  <arg name="move_base_config" value="2d" />
-  <arg name="pointcloud_topic" default="/camera/depth/color/points"/>
-  <arg name="cmd_vel_topic" default="/stretch_diff_drive_controller/cmd_vel"/>
-  <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
-  
-  <!-- Navigation stuff (move_base) -->
-  <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
-      <arg name="config" value="$(arg move_base_config)" />
-      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)" />
-      <arg name="odom_topic" value="$(arg odom_topic)" />
-  </include>
-  
-  <!-- Mapping -->
-  <group ns="rtabmap">
-
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
-      <param name="database_path"       type="string" value="$(arg database_path)"/>
-      <param name="frame_id"            type="string" value="base_link"/>
-      <param name="odom_frame_id"       type="string" value="odom"/>
-      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
-      <param name="subscribe_depth"     type="bool"   value="true"/>
-      <param name="subscribe_scan"      type="bool"   value="true"/>
-    
-      <!-- inputs -->
-      <remap from="scan"            to="/scan"/>
-      <remap from="rgb/image"       to="/camera/color/image_raw"/>
-      <remap from="depth/image"     to="/camera/depth/image_raw"/>
-      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
-      
-      <!-- output -->
-      <remap from="grid_map" to="/map"/>
-    
-      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   
-      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  
-      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
-      <param name="Reg/Strategy"                 type="string" value="1"/>      
-      <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
-      <param name="Vis/MinInliers"               type="string" value="6"/>      
-      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    
-      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    
-      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    
-      <param name="Rtabmap/TimeThr"              type="string" value="700"/>
-      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
-      <param name="Optimizer/Slam2D"             type="string" value="true"/>
-      <param name="Reg/Force3DoF"                type="string" value="true"/>   
-      
-    </node>
-   
-    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
-    <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
-    <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
-  </group>
-</launch>
\ No newline at end of file
diff --git a/stretch_navigation/launch/rtab/robot.launch b/stretch_navigation/launch/rtab/robot.launch
deleted file mode 100644
index 80ee9f4..0000000
--- a/stretch_navigation/launch/rtab/robot.launch
+++ /dev/null
@@ -1,36 +0,0 @@
-<launch>
-    <!-- REALSENSE D435i -->
-    <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
-    <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
-    <!--<param name="initial_mode" type="string" value="Default"/>-->
-    <param name="initial_mode" type="string" value="High Accuracy"/>
-    </node>
-    <!-- -->
-
-    <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
-    <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" />
-    <!-- -->
-
-    <!-- STRETCH DRIVER -->
-    <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
-    <param name="/stretch_driver/mode" type="string" value="navigation" />
-    <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
-    <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
-    <!-- -->
-
-    <!-- IMU FILTER -->
-    <!-- <include file="$(find stretch_core)/launch/imu_filter.launch" /> -->
-    <!-- -->
-
-    <!-- LASER RANGE FINDER -->
-    <include file="$(find stretch_core)/launch/rplidar.launch" />
-    <!-- -->
-
-    <!-- LASER SCAN MATCHER FOR ODOMETRY -->
-    <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" />
-    <!-- -->
-
-    <!-- ROBOT LOCALIZATION FILTER -->
-    <include file="$(find stretch_core)/launch/stretch_ekf.launch" />
-    <!-- -->
-</launch>
\ No newline at end of file
diff --git a/stretch_navigation/launch/rtab/rtab.launch b/stretch_navigation/launch/rtab/rtab.launch
deleted file mode 100644
index d67d404..0000000
--- a/stretch_navigation/launch/rtab/rtab.launch
+++ /dev/null
@@ -1,57 +0,0 @@
-<launch>
-  <arg name="localization"/>
-  <arg name="args"/>
-  <arg name="database_path"/>
-  <arg name="wait_for_transform"/> 
-  <arg name="pointcloud_topic"/>
-  <arg name="cmd_vel_topic"/>
-  <arg name="odom_topic"/>
-  <arg name="scan"/>
-  <arg name="rgb_topic"/>
-  <arg name="rgb_camera_info"/>
-  <!-- Mapping -->
-  <group ns="rtabmap">
-
-    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
-      <param name="database_path"       type="string" value="$(arg database_path)"/>
-      <param name="frame_id"            type="string" value="base_link"/>
-      <param name="odom_frame_id"       type="string" value="odom"/>
-      <param name="odom_tf_angular_variance" type="double" value="0.005"/>
-      <param name="odom_tf_linear_variance"  type="double" value="0.005"/>
-      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
-      <param name="subscribe_depth"     type="bool"   value="true"/>
-      <param name="subscribe_scan"      type="bool"   value="true"/>
-    
-      <!-- inputs -->
-      <remap from="scan"            to="$(arg scan)"/>
-      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
-      <remap from="depth/image"     to="$(arg pointcloud_topic)"/>
-      <remap from="rgb/camera_info" to="$(arg rgb_camera_info)"/>
-      
-      <!-- output -->
-      <remap from="grid_map" to="/map"/>
-    
-      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
-      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   
-      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  
-      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
-      <param name="Reg/Strategy"                 type="string" value="1"/>      
-      <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
-      <param name="Vis/MinInliers"               type="string" value="6"/>      
-      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    
-      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    
-      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    
-      <param name="Rtabmap/TimeThr"              type="string" value="700"/>
-      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
-      <param name="Reg/Force3DoF"                type="string" value="true"/>  
-      <param name="Grid/FromDepth"               type="string" value="true"/>
-      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> 
-      
-    </node>
-    
-    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
-    <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
-    <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
-  </group>
-
-</launch>
\ No newline at end of file
diff --git a/stretch_navigation/launch/rtab_generic.launch b/stretch_navigation/launch/rtab_generic.launch
deleted file mode 100644
index b4e98bb..0000000
--- a/stretch_navigation/launch/rtab_generic.launch
+++ /dev/null
@@ -1,65 +0,0 @@
-<launch>
-  <arg name="sim" />
-  <arg name="localization" default="false" />
-
-  <arg name="database_path"     default="rtabmap.db"/>
-  <arg name="args"              default=""/>
-
-  <arg name="wait_for_transform"  default="0.2"/> 
-  <arg name="move_base_config" value="2d" />
-
-
-  <group if="$(arg sim)">
-    <arg name="pointcloud_topic" default="/camera/depth/image_raw"/>
-    <arg name="cmd_vel_topic" value="/stretch_diff_drive_controller/cmd_vel"/>
-    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
-    <arg name="scan_topic" default="/scan"/>
-    <arg name="rgb_topic" default="/camera/color/image_raw"/>
-    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
-
-    <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
-      <arg name="config" value="$(arg move_base_config)" />
-      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
-      <arg name="odom_topic" value="$(arg odom_topic)"/>
-    </include>
-    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
-      <arg name="localization" value="$(arg localization)"/>
-      <arg name="args" value="$(arg args)"/>
-      <arg name="database_path"     default="$(arg database_path)"/>
-      <arg name="wait_for_transform"  default="$(arg wait_for_transform)"/> 
-      <arg name="pointcloud_topic" value="$(arg pointcloud_topic)"/>
-      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
-      <arg name="odom_topic" default="$(arg odom_topic)"/>
-      <arg name="scan" default="$(arg scan_topic)"/>
-      <arg name="rgb_topic" default="$(arg rgb_topic)"/>
-      <arg name="rgb_camera_info" default="$(arg rgb_camera_info)"/>
-    </include>
-  </group>
-  <group unless="$(arg sim)">
-    <arg name="pointcloud_topic" value="/camera/aligned_depth_to_color/image_raw"/>
-    <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
-    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
-    <arg name="scan_topic" default="/scan"/>
-    <arg name="rgb_topic" default="/camera/color/image_raw"/>
-    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
-
-    <include file="$(find stretch_navigation)/launch/rtab/robot.launch"/>
-    <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
-      <arg name="config" value="$(arg move_base_config)" />
-      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
-      <arg name="odom_topic" value="$(arg odom_topic)"/>
-    </include>
-    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
-      <arg name="localization" value="$(arg localization)"/>
-      <arg name="args" value="$(arg args)"/>
-      <arg name="database_path"     default="$(arg database_path)"/>
-      <arg name="wait_for_transform"  default="$(arg wait_for_transform)"/> 
-      <arg name="pointcloud_topic" value="$(arg pointcloud_topic)"/>
-      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
-      <arg name="odom_topic" default="$(arg odom_topic)"/>
-      <arg name="scan" default="$(arg scan_topic)"/>
-      <arg name="rgb_topic" default="$(arg rgb_topic)"/>
-      <arg name="rgb_camera_info" default="$(arg rgb_camera_info)"/>
-    </include>
-  </group>
-</launch>
\ No newline at end of file
diff --git a/stretch_navigation/launch/rtab_robot.launch b/stretch_navigation/launch/rtab_robot.launch
deleted file mode 100644
index f0427fb..0000000
--- a/stretch_navigation/launch/rtab_robot.launch
+++ /dev/null
@@ -1,51 +0,0 @@
-<launch>
-  <arg name="sim" />
-  <arg name="localization" default="false" />
-
-  <arg name="database_path"     default="rtabmap.db"/>
-  <arg name="args"              default=""/>
-
-  <arg name="wait_for_transform"  default="0.2"/> 
-  <arg name="move_base_config" value="2d" />
-
-
-  <group if="$(arg sim)">
-    <arg name="pointcloud_topic" default="/camera/depth/color/points"/>
-    <arg name="cmd_vel_topic" value="/stretch_diff_drive_controller/cmd_vel"/>
-    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
-    <arg name="scan_topic" default="/scan"/>
-    <arg name="rgb_topic" default="/camera/color/image_raw"/>
-    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
-
-    <include file="$(find stretch_navigation)/launch/rtab/move_base.launch">
-      <arg name="move_base_config" value="$(arg move_base_config)" />
-    </include>
-    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
-      <arg name="localization" value="$(arg localization)"/>
-    </include>
-  </group>
-  <group unless="$(arg sim)">
-    <arg name="pointcloud_topic" value="/camera/aligned_depth_to_color/image_raw"/>
-    <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
-    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
-    <arg name="scan_topic" default="/scan"/>
-    <arg name="rgb_topic" default="/camera/color/image_raw"/>
-    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
-
-    <include file="$(find stretch_navigation)/launch/rtab/robot.launch"/>
-    <include file="$(find stretch_navigation)/launch/rtab/move_base.launch">
-      <arg name="move_base_config" value="$(arg move_base_config)" />
-      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
-      <arg name="odom_topic" value="$(arg odom_topic)"/>
-    </include>
-    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
-      <arg name="localization" value="$(arg localization)"/>
-      <arg name="pointcloud_topic" value="/camera/aligned_depth_to_color/image_raw"/>
-      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
-      <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
-      <arg name="scan_topic" default="/scan"/>
-      <arg name="rgb_topic" default="/camera/color/image_raw"/>
-      <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
-    </include>
-  </group>
-</launch>
\ No newline at end of file
diff --git a/stretch_navigation/launch/rviz_rtab.launch b/stretch_navigation/launch/rviz_rtab.launch
deleted file mode 100644
index 1a7b984..0000000
--- a/stretch_navigation/launch/rviz_rtab.launch
+++ /dev/null
@@ -1,3 +0,0 @@
-<launch>
-  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find stretch_navigation)/rviz/rtab_mapping.rviz"/>
-</launch>
\ No newline at end of file
diff --git a/stretch_navigation/launch/teleop.launch b/stretch_navigation/launch/teleop.launch
deleted file mode 100644
index 228e44c..0000000
--- a/stretch_navigation/launch/teleop.launch
+++ /dev/null
@@ -1,7 +0,0 @@
-<launch>
-  <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" >
-    <param name="speed" type="double" value="0.04" />
-    <param name="turn" type="double" value="0.1" />
-    <remap from="/cmd_vel" to="/stretch/cmd_vel" />
-  </node>
-</launch>
\ No newline at end of file
diff --git a/stretch_navigation/launch/rtab_gazebo.launch b/stretch_rtab/launch/rtab_gazebo.launch
similarity index 100%
rename from stretch_navigation/launch/rtab_gazebo.launch
rename to stretch_rtab/launch/rtab_gazebo.launch
diff --git a/stretch_navigation/launch/rtabmapviz.launch b/stretch_rtab/launch/rtabmapviz.launch
similarity index 100%
rename from stretch_navigation/launch/rtabmapviz.launch
rename to stretch_rtab/launch/rtabmapviz.launch