From 5f87acfa38303089567e2c4222d15c5e65601ff8 Mon Sep 17 00:00:00 2001
From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Fri, 29 Dec 2023 13:50:29 -0300
Subject: [PATCH] added rtabmap slam(uncalibrated)

---
 twil_2dnav/launch/gazebo.launch.xml                |  2 +-
 twil_2dnav/launch/localization_amcl.launch.xml     | 12 ------
 twil_2dnav/launch/localization_rtabmap.launch.xml  | 32 ++++++++++++++
 .../launch/localization_slam_toolbox.launch.xml    | 11 -----
 twil_2dnav/launch/localization_static.launch.xml   | 11 -----
 twil_2dnav/launch/nav2_navigator.launch.xml        | 49 +++++++++++++++-------
 twil_2dnav/launch/odom_controller.launch.xml       |  2 +
 .../launch/odom_noisy_ground_truth.launch.xml      |  3 ++
 twil_2dnav/launch/odom_rtabmap_viodom.launch.xml   |  5 +++
 9 files changed, 77 insertions(+), 50 deletions(-)
 create mode 100644 twil_2dnav/launch/localization_rtabmap.launch.xml

diff --git a/twil_2dnav/launch/gazebo.launch.xml b/twil_2dnav/launch/gazebo.launch.xml
index 88299ce..280cb91 100644
--- a/twil_2dnav/launch/gazebo.launch.xml
+++ b/twil_2dnav/launch/gazebo.launch.xml
@@ -27,7 +27,7 @@
 	<arg name="gps" default="false"/>
 	<arg name="world" default ="$(find-pkg-share ufrgs_gazebo)/worlds/centenario.world"/>
 	<arg name="controller" default="twist_mrac_linearizing_controller"/>
-	<arg name="odom" default="rtabmap_viodom"/>
+	<arg name="odom" default="noisy_ground_truth"/>
 	<arg name="localization" default="static"/>
 	<arg name="controller_config" default="$(find-pkg-share twil_2dnav)/config/$(var controller)_no_tf.yaml"/>
 
diff --git a/twil_2dnav/launch/localization_amcl.launch.xml b/twil_2dnav/launch/localization_amcl.launch.xml
index d695f0c..c54b914 100644
--- a/twil_2dnav/launch/localization_amcl.launch.xml
+++ b/twil_2dnav/launch/localization_amcl.launch.xml
@@ -5,17 +5,5 @@
         <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/localization_rtabmap.launch.xml b/twil_2dnav/launch/localization_rtabmap.launch.xml
new file mode 100644
index 0000000..36e556a
--- /dev/null
+++ b/twil_2dnav/launch/localization_rtabmap.launch.xml
@@ -0,0 +1,32 @@
+<launch>
+    <arg name="use_sim_time" default="false"/>
+
+    <node name="rtabmap" pkg="rtabmap_slam" exec="rtabmap" args="--delete_db_on_start" namespace="rtabmap">
+          <param name="use_sim_time" value="$(var use_sim_time)"/>
+          <param name="frame_id" value="twil_origin"/>
+          <!-- <param name="odom_frame_id" value="odom"/> -->
+
+          <remap from="odom" to="/odom"/>
+
+          <param name="subscribe_depth" value="true"/>
+          <remap from="depth/image" to="/camera/depth/image_raw"/>
+          <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
+          <remap from="rgb/image" to="/camera/color/image_raw"/>
+
+          <!-- <param name="subscribe_scan" value="true"/>
+          <remap from="scan" to="/camera/depth/image_scan"/> -->
+
+          <!-- <param name="queue_size" value="50"/> -->
+
+          <!-- <param name="tf_delay" value="0.1"/> -->
+          <!-- <param name="wait_for_transform_duration" value="5.0"/> -->
+          <!-- RTAB-Map's parameters -->
+          <!-- <param name="RGBD/ProximityBySpace" value="false"/>
+          <param name="RGBD/AngularUpdate" value="0.01"/>
+          <param name="RGBD/LinearUpdate" value="0.01"/>
+          <param name="RGBD/OptimizeFromGraphEnd" value="false"/>
+          <param name="Reg/Force3DoF" value="true"/>
+          <param name="Vis/MinInliers" value="12"/> -->
+    </node>
+
+</launch>
diff --git a/twil_2dnav/launch/localization_slam_toolbox.launch.xml b/twil_2dnav/launch/localization_slam_toolbox.launch.xml
index b2dae96..6619894 100644
--- a/twil_2dnav/launch/localization_slam_toolbox.launch.xml
+++ b/twil_2dnav/launch/localization_slam_toolbox.launch.xml
@@ -6,15 +6,4 @@
         <param from="$(find-pkg-share twil_2dnav)/config/slam_toolbox_mapping_params.yaml"/>
         <remap from="map" to="slam_map"/> 
     </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',
-            'controller_server',
-            'planner_server',
-            'behavior_server',
-            'bt_navigator']"/>
-    </node>
 </launch>
diff --git a/twil_2dnav/launch/localization_static.launch.xml b/twil_2dnav/launch/localization_static.launch.xml
index edbfed2..73cafac 100644
--- a/twil_2dnav/launch/localization_static.launch.xml
+++ b/twil_2dnav/launch/localization_static.launch.xml
@@ -3,16 +3,5 @@
 
     <node pkg="tf2_ros" exec="static_transform_publisher" name="odom_frame_publisher" 
         args="--frame-id map --child-frame-id odom"/>
-
-    <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',
-            '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 c541cc4..f86e877 100644
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -24,8 +24,24 @@
     <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="static"/>
+    <arg name="localization" default=""/>
     <arg name="odom" default="rtabmap_viodom"/>
+    <let name="amcl" value="$(eval '\'$(var localization)\' == \'amcl\'')" />
+
+	<include file="$(find-pkg-share twil_2dnav)/launch/localization_$(var localization).launch.xml">
+        <arg name="use_sim_time" value="$(var use_sim_time)"/>
+	</include>
+
+	<include file="$(find-pkg-share twil_2dnav)/launch/odom_$(var odom).launch.xml">
+        <arg name="controller" value="$(var controller)"/>
+	</include>
+
+    <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" value="camera_link"/> 
+    </node>
 
     <node pkg="nav2_controller" exec="controller_server" name="controller_server">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
@@ -45,7 +61,6 @@
         <param name="robot_base_frame" value="twil_origin"/>
     </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,19 +71,23 @@
         <param name="yaml_filename" value="$(find-pkg-share ufrgs_maps)/map/centenario1.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"/> 
+    <let name="nav2_nodes" if="$(var amcl)" value="[
+        'map_server',
+        'amcl',
+        'controller_server',
+        'planner_server',
+        'behavior_server',
+        'bt_navigator']"/>
+    <let name="nav2_nodes" unless="$(var amcl)" value="[
+        'map_server',
+        'controller_server',
+        'planner_server',
+        'behavior_server',
+        'bt_navigator']"/>
+    <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="$(var nav2_nodes)"/>
     </node>
 
-	<include file="$(find-pkg-share twil_2dnav)/launch/localization_$(var localization).launch.xml">
-        <arg name="use_sim_time" value="$(var use_sim_time)"/>
-	</include>
-
-	<include file="$(find-pkg-share twil_2dnav)/launch/odom_$(var odom).launch.xml">
-        <arg name="controller" value="$(var controller)"/>
-	</include>
-
 </launch>
diff --git a/twil_2dnav/launch/odom_controller.launch.xml b/twil_2dnav/launch/odom_controller.launch.xml
index bd5ec33..316e635 100644
--- a/twil_2dnav/launch/odom_controller.launch.xml
+++ b/twil_2dnav/launch/odom_controller.launch.xml
@@ -1,7 +1,9 @@
 <launch>
     <arg name="controller" default=""/>
+    <arg name="use_sim_time" default="false"/>
 
     <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
         <remap from="odometry/filtered" to="odom"/>
         <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param name="odom0" value="$(var controller)/odom"/>
diff --git a/twil_2dnav/launch/odom_noisy_ground_truth.launch.xml b/twil_2dnav/launch/odom_noisy_ground_truth.launch.xml
index 7fe12c3..95d5fc3 100644
--- a/twil_2dnav/launch/odom_noisy_ground_truth.launch.xml
+++ b/twil_2dnav/launch/odom_noisy_ground_truth.launch.xml
@@ -1,5 +1,8 @@
 <launch>
+    <arg name="use_sim_time" default="false"/>
+
     <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
         <remap from="odometry/filtered" to="odom"/>
         <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param name="odom0" value="/fake_odom"/>
diff --git a/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml b/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
index 1ad0717..8bceecb 100644
--- a/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
+++ b/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
@@ -1,6 +1,11 @@
 
 <launch>
+    <arg name="use_sim_time" default="false"/>
+
     <node pkg="rtabmap_odom" exec="rgbd_odometry" name="rgbd_odometry" output="screen">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param name="frame_id" value="twil_origin"/>
         <param name="odom_frame_id" value="odom"/>
         <param name="scan_normal_k" value="10"/>
-- 
2.12.0