From 12a5f85d6de92ebaf717f627ce125ae3d30565dc Mon Sep 17 00:00:00 2001
From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Sat, 30 Dec 2023 14:57:20 -0300
Subject: [PATCH] added odom and locallization to bag playback

---
 twil_2dnav/config/{bag.rviz => bag_playback.rviz} |  0
 twil_2dnav/launch/bag_playback.launch.xml         | 24 +++++++++++++++++++++--
 twil_2dnav/launch/localization_static.launch.xml  |  2 +-
 twil_2dnav/launch/odom_rtabmap_viodom.launch.xml  |  1 -
 twil_2dnav/scripts/record_bag.sh                  |  2 +-
 5 files changed, 24 insertions(+), 5 deletions(-)
 rename twil_2dnav/config/{bag.rviz => bag_playback.rviz} (100%)

diff --git a/twil_2dnav/config/bag.rviz b/twil_2dnav/config/bag_playback.rviz
similarity index 100%
rename from twil_2dnav/config/bag.rviz
rename to twil_2dnav/config/bag_playback.rviz
diff --git a/twil_2dnav/launch/bag_playback.launch.xml b/twil_2dnav/launch/bag_playback.launch.xml
index 8aaa854..f50f507 100644
--- a/twil_2dnav/launch/bag_playback.launch.xml
+++ b/twil_2dnav/launch/bag_playback.launch.xml
@@ -1,7 +1,27 @@
 <launch>
-	<arg name="use_sim_time" default="true"/>
+	<arg name="use_sim_time" default="false"/>
+	<arg name="odom" default="noisy_ground_truth"/>
+	<arg name="localization" default="static"/>
 
-	<node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share twil_2dnav)/config/bag.rviz">
+	<node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share twil_2dnav)/config/bag_playback.rviz">
 		<param name="use_sim_time" value="$(var use_sim_time)"/>
 	</node>
+
+    <!-- tf twil_origin->sensors -->
+	<include file="$(find-pkg-share twil_description)/launch/twil.launch.xml">
+		<arg name="wam" value="false" />
+		<arg name="d435" value="true" />
+	</include>
+
+    <!-- tf odom->twil_origin -->
+	<include file="$(find-pkg-share twil_2dnav)/launch/odom_$(var odom).launch.xml">
+        <arg name="controller" value="twist_mrac_controller"/>
+		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+	</include>
+
+    <!-- tf map->odom -->
+	<include file="$(find-pkg-share twil_2dnav)/launch/localization_$(var localization).launch.xml">
+		<arg name="use_sim_time" value="$(var use_sim_time)"/>
+	</include>
+
 </launch>
diff --git a/twil_2dnav/launch/localization_static.launch.xml b/twil_2dnav/launch/localization_static.launch.xml
index 73cafac..0071037 100644
--- a/twil_2dnav/launch/localization_static.launch.xml
+++ b/twil_2dnav/launch/localization_static.launch.xml
@@ -1,7 +1,7 @@
 <launch>
     <arg name="use_sim_time" default="false"/>
 
-    <node pkg="tf2_ros" exec="static_transform_publisher" name="odom_frame_publisher" 
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="map_odom_tf_publisher" 
         args="--frame-id map --child-frame-id odom"/>
 </launch>
 
diff --git a/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml b/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
index 8bceecb..5e16d64 100644
--- a/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
+++ b/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
@@ -5,7 +5,6 @@
     <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"/>
diff --git a/twil_2dnav/scripts/record_bag.sh b/twil_2dnav/scripts/record_bag.sh
index 042af41..81f4cfc 100755
--- a/twil_2dnav/scripts/record_bag.sh
+++ b/twil_2dnav/scripts/record_bag.sh
@@ -11,7 +11,7 @@ output_bag_filename=$1
 
 # Define the local variable with the desired topic names
 camera_topics="/camera/depth/image_scan /camera/depth/camera_info /camera/depth/points /camera/depth/image_raw /camera/color/camera_info /camera/color/image_raw"
-robot_description_topics="/clock /tf /tf_static /robot_description /ground_truth"
+robot_description_topics="/joint_states /ground_truth"
 sensor_topics="/odom /sensor/imu /fake_odom /twist_mrac_controller/odom"
 
 qos_overrides_file="qos_overrides.yaml"
-- 
2.12.0