From 13fa495bec741a7454d92930250645779071c183 Mon Sep 17 00:00:00 2001
From: hello-binit <64861565+hello-binit@users.noreply.github.com>
Date: Sat, 31 Jul 2021 02:54:11 -0400
Subject: [PATCH] Add simulated navigation to Stretch Navigation

---
 .../launch/navigation_gazebo.launch           | 35 +++++++++++++++++++
 1 file changed, 35 insertions(+)
 create mode 100644 stretch_navigation/launch/navigation_gazebo.launch

diff --git a/stretch_navigation/launch/navigation_gazebo.launch b/stretch_navigation/launch/navigation_gazebo.launch
new file mode 100644
index 0000000..d5cc933
--- /dev/null
+++ b/stretch_navigation/launch/navigation_gazebo.launch
@@ -0,0 +1,35 @@
+<launch>
+
+  <arg name="map_yaml" />
+  <arg name="rviz" default="true"/>
+  <arg name="gazebo_gpu_lidar" default="false"/>
+  <arg name="gazebo_visualize_lidar" default="false"/>
+  <arg name="gazebo_world" default="worlds/empty.world"/>
+
+  <!-- GAZEBO SIMULATION -->
+  <include file="$(find stretch_gazebo)/launch/gazebo.launch">
+    <arg name="world" value="$(arg gazebo_world)" />
+    <arg name="visualize_lidar" value="$(arg gazebo_visualize_lidar)" />
+    <arg name="gpu_lidar" value="$(arg gazebo_gpu_lidar)" />
+  </include>
+
+  <!-- MAP SERVER -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_yaml)" />
+
+  <!-- AMCL -->
+  <include file="$(find amcl)/examples/amcl_diff.launch" />
+
+  <!-- MOVE BASE -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <rosparam file="$(find stretch_navigation)/config/common_costmap_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stretch_navigation)/config/common_costmap_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stretch_navigation)/config/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/config/base_local_planner_params.yaml" command="load" />
+    <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
+  </node>
+
+  <!-- RVIZ -->
+  <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/navigation.rviz" if="$(arg rviz)" />
+
+</launch>