From fbbdc5ab2f0dead91a66579a26fcb650e20379bf Mon Sep 17 00:00:00 2001
From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Fri, 3 Nov 2023 21:27:26 -0300
Subject: [PATCH] separated amcl related nodes in another launch file

this was made to prepare for the addition of the other localization
methods.
maybe a python launch file would be better, for example, to set
the node names of the lifecycle manager node
---
 twil_2dnav/launch/localization_amcl.launch.xml | 26 +++++++++++++++++++++++
 twil_2dnav/launch/nav2_navigator.launch.xml    | 29 +++++---------------------
 2 files changed, 31 insertions(+), 24 deletions(-)
 create mode 100644 twil_2dnav/launch/localization_amcl.launch.xml

diff --git a/twil_2dnav/launch/localization_amcl.launch.xml b/twil_2dnav/launch/localization_amcl.launch.xml
new file mode 100644
index 0000000..e8a3229
--- /dev/null
+++ b/twil_2dnav/launch/localization_amcl.launch.xml
@@ -0,0 +1,26 @@
+<launch>
+    <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" exec="depthimage_to_laserscan_node">
+        <remap from="depth" to="/camera/depth/image_raw"/> 
+        <remap from="depth_camera_info" to="/camera/depth/camera_info"/> 
+        <remap from="scan" to="/camera/depth/image_scan"/> 
+        <param name="output_frame" type="str" value="camera_link"/> 
+    </node>
+
+    <node pkg="nav2_amcl" exec="amcl" name="amcl">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param from="$(find-pkg-share twil_2dnav)/config/amcl_params.yaml"/>
+    </node>
+
+    <node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager_nav2">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="autostart" value="$(var autostart)"/>
+        <param name="node_names" value="[
+            'map_server',
+            'amcl',
+            'controller_server',
+            'planner_server',
+            'behavior_server',
+            'bt_navigator']"/>
+    </node>
+</launch>
+
diff --git a/twil_2dnav/launch/nav2_navigator.launch.xml b/twil_2dnav/launch/nav2_navigator.launch.xml
index 0cf56b3..d303822 100644
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -24,6 +24,7 @@
     <arg name="map" default="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
     <arg name="autostart" default="true"/>
     <arg name="controller" default=""/>
+    <arg name="localization" default="amcl"/>
 
     <node pkg="nav2_map_server" name="map_server" exec="map_server">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
@@ -49,6 +50,7 @@
         <remap from="cmd_vel" to="$(var controller)/command"/>
     </node>
 
+
     <node pkg="nav2_bt_navigator" exec="bt_navigator" name="bt_navigator">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param from="$(find-pkg-share twil_2dnav)/config/bt_navigator_params.yaml"/>
@@ -56,34 +58,13 @@
         <remap from="odom" to="$(var controller)/odom"/>
     </node>
 
-    <node pkg="nav2_amcl" exec="amcl" name="amcl">
-        <param name="use_sim_time" value="$(var use_sim_time)"/>
-        <param from="$(find-pkg-share twil_2dnav)/config/amcl_params.yaml"/>
-    </node>
-
-    <node pkg="nav2_lifecycle_manager" exec="lifecycle_manager" name="lifecycle_manager_nav2">
-        <param name="use_sim_time" value="$(var use_sim_time)"/>
-        <param name="autostart" value="$(var autostart)"/>
-        <param name="node_names" value="[
-            'map_server',
-            'amcl',
-			'controller_server',
-			'planner_server',
-			'behavior_server',
-			'bt_navigator']"/>
-    </node>
-
-    <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node" output="screen">
+    <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param name="odom0" value="$(var controller)/odom"/>
         <param from="$(find-pkg-share twil_2dnav)/config/ekf_params.yaml"/>
     </node>
 
-    <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" exec="depthimage_to_laserscan_node">
-        <remap from="depth" to="/camera/depth/image_raw"/> 
-        <remap from="depth_camera_info" to="/camera/depth/camera_info"/> 
-        <remap from="scan" to="/camera/depth/image_scan"/> 
-        <param name="output_frame" type="str" value="camera_link"/> 
-    </node>
+	<include file="$(find-pkg-share twil_2dnav)/launch/localization_$(var localization).launch.xml">
+	</include>
 
 </launch>
-- 
2.12.0