From 6d41a43eff1a348a6956ffe56e5f47d143df1f13 Mon Sep 17 00:00:00 2001
From: Henrique Scharlau Coelho <henriquescharlaucoelho@gmail.com>
Date: Sat, 30 Dec 2023 18:07:25 -0300
Subject: [PATCH] added perfect odom launch option and fixed bag recording

---
 twil_2dnav/config/bag_playback.rviz               | 267 +++++++++++++++++++++-
 twil_2dnav/config/planner_params.yaml             |   2 +-
 twil_2dnav/launch/bag_playback.launch.xml         |   2 +-
 twil_2dnav/launch/gazebo.launch.xml               |   2 +-
 twil_2dnav/launch/localization_rtabmap.launch.xml |   7 +-
 twil_2dnav/launch/odom_perfect.launch.xml         |  17 ++
 twil_2dnav/scripts/record_bag.sh                  |   4 +-
 7 files changed, 292 insertions(+), 9 deletions(-)
 create mode 100644 twil_2dnav/launch/odom_perfect.launch.xml

diff --git a/twil_2dnav/config/bag_playback.rviz b/twil_2dnav/config/bag_playback.rviz
index d968437..368fa20 100644
--- a/twil_2dnav/config/bag_playback.rviz
+++ b/twil_2dnav/config/bag_playback.rviz
@@ -23,7 +23,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: LaserScan
 Visualization Manager:
   Class: ""
   Displays:
@@ -50,13 +50,117 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
+        battery:
+          Value: true
+        bottom_sonar_box:
+          Value: true
+        bottom_sonar_cover:
+          Value: true
+        bottom_sonar_sensor:
+          Value: true
+        camera_bottom_screw_frame:
+          Value: true
+        camera_link:
+          Value: true
+        camera_optical_frame:
+          Value: true
+        caster_base:
+          Value: true
+        caster_support:
+          Value: true
+        caster_wheel:
+          Value: true
+        chassis:
+          Value: true
+        chassis_top:
+          Value: true
+        cpu:
+          Value: true
+        fan:
+          Value: true
+        imu:
+          Value: true
+        left_wheel:
+          Value: true
+        left_wheel_support:
+          Value: true
+        map:
+          Value: true
+        motor_driver:
+          Value: true
+        odom:
+          Value: true
+        power_supply:
+          Value: true
+        right_wheel:
+          Value: true
+        right_wheel_support:
+          Value: true
+        stepper_driver:
+          Value: true
+        top_sonar_box:
+          Value: true
+        top_sonar_cover:
+          Value: true
+        top_sonar_sensor:
+          Value: true
+        tower:
+          Value: true
+        tower_motor:
+          Value: true
+        twil_origin:
+          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        map:
+          odom:
+            twil_origin:
+              chassis:
+                battery:
+                  {}
+                caster_support:
+                  caster_base:
+                    caster_wheel:
+                      {}
+                chassis_top:
+                  {}
+                cpu:
+                  {}
+                fan:
+                  {}
+                imu:
+                  {}
+                left_wheel_support:
+                  left_wheel:
+                    {}
+                motor_driver:
+                  {}
+                power_supply:
+                  {}
+                right_wheel_support:
+                  right_wheel:
+                    {}
+                stepper_driver:
+                  {}
+                tower:
+                  bottom_sonar_box:
+                    bottom_sonar_cover:
+                      bottom_sonar_sensor:
+                        {}
+                  camera_bottom_screw_frame:
+                    camera_link:
+                      camera_optical_frame:
+                        {}
+                  top_sonar_box:
+                    top_sonar_cover:
+                      top_sonar_sensor:
+                        {}
+                tower_motor:
+                  {}
       Update Interval: 0
       Value: true
     - Alpha: 1
@@ -77,6 +181,142 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        battery:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bottom_sonar_box:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bottom_sonar_cover:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        bottom_sonar_sensor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        caster_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_support:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        chassis:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        chassis_top:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cpu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        fan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        imu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        left_wheel_support:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        motor_driver:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        power_supply:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        right_wheel_support:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        stepper_driver:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        top_sonar_box:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        top_sonar_cover:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        top_sonar_sensor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tower:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        tower_motor:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        twil_origin:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
@@ -211,6 +451,7 @@ Visualization Manager:
         Value: /camera/depth/image_raw
       Value: false
       Visibility:
+        "": true
         Camera color: true
         Grid: true
         Ground_truth: true
@@ -236,6 +477,7 @@ Visualization Manager:
         Value: /camera/color/image_raw
       Value: false
       Visibility:
+        "": true
         Camera depth: true
         Grid: true
         Ground_truth: true
@@ -295,6 +537,27 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz_default_plugins/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /slam_map
+      Update Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /slam_map_updates
+      Use Timestamp: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
diff --git a/twil_2dnav/config/planner_params.yaml b/twil_2dnav/config/planner_params.yaml
index 368902d..ba14fab 100644
--- a/twil_2dnav/config/planner_params.yaml
+++ b/twil_2dnav/config/planner_params.yaml
@@ -1,6 +1,6 @@
 planner_server:
   ros__parameters:
-    expected_planner_frequency: 20.0
+    expected_planner_frequency: 10.0
     planner_plugins: ["GridBased"]
     GridBased:
       plugin: "nav2_navfn_planner/NavfnPlanner"
diff --git a/twil_2dnav/launch/bag_playback.launch.xml b/twil_2dnav/launch/bag_playback.launch.xml
index f50f507..cc5c41e 100644
--- a/twil_2dnav/launch/bag_playback.launch.xml
+++ b/twil_2dnav/launch/bag_playback.launch.xml
@@ -1,5 +1,5 @@
 <launch>
-	<arg name="use_sim_time" default="false"/>
+	<arg name="use_sim_time" default="true"/>
 	<arg name="odom" default="noisy_ground_truth"/>
 	<arg name="localization" default="static"/>
 
diff --git a/twil_2dnav/launch/gazebo.launch.xml b/twil_2dnav/launch/gazebo.launch.xml
index 280cb91..6d93f93 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="noisy_ground_truth"/>
+	<arg name="odom" default="perfect"/>
 	<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_rtabmap.launch.xml b/twil_2dnav/launch/localization_rtabmap.launch.xml
index 36e556a..4141858 100644
--- a/twil_2dnav/launch/localization_rtabmap.launch.xml
+++ b/twil_2dnav/launch/localization_rtabmap.launch.xml
@@ -4,15 +4,18 @@
     <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"/>
+          <!-- escolhi usar o frame_id porque o odom_perfect nao publica no /odom -->
+          <!-- caso seja adicionada a publicacao do /odom nele, seria melhor usar o topico ao inves do frame_id para usar a info de covariancia -->
+          <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"/>
 
+          <remap from="map" to="/slam_map"/>
           <!-- <param name="subscribe_scan" value="true"/>
           <remap from="scan" to="/camera/depth/image_scan"/> -->
 
diff --git a/twil_2dnav/launch/odom_perfect.launch.xml b/twil_2dnav/launch/odom_perfect.launch.xml
new file mode 100644
index 0000000..3af0b43
--- /dev/null
+++ b/twil_2dnav/launch/odom_perfect.launch.xml
@@ -0,0 +1,17 @@
+<launch>
+    <arg name="use_sim_time" default="true"/>
+
+    <!-- utiliza o pacote odom_to_tf_ros2 para criar uma transformada odom->twil_origin a partir do ground_truth -->
+    <!-- é pra ser utilizado com a localizacao estatica pra ter uma localizaçao "perfeita" do robo -->
+    <!-- é util para, por exemplo, criar um bag para depois comparar os outros odom/localization -->
+
+    <!-- seria uma boa adicionar a publicaçao no topico /odom -->
+    <!-- teria que pegar o ground_truth e mudar o header dele para o frame_id=odom e publicar o resto igual no topico /odom -->
+
+    <node pkg="odom_to_tf_ros2" exec="odom_to_tf" name="odom_to_tf">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="odom_topic" value="/ground_truth"/>
+        <param name="frame_id" value="odom"/>
+        <param name="child_frame_id" value="twil_origin"/>
+    </node>
+</launch>
diff --git a/twil_2dnav/scripts/record_bag.sh b/twil_2dnav/scripts/record_bag.sh
index 81f4cfc..bb7b972 100755
--- a/twil_2dnav/scripts/record_bag.sh
+++ b/twil_2dnav/scripts/record_bag.sh
@@ -11,8 +11,8 @@ 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="/joint_states /ground_truth"
-sensor_topics="/odom /sensor/imu /fake_odom /twist_mrac_controller/odom"
+robot_description_topics="/clock /joint_states /ground_truth"
+sensor_topics="/sensor/imu /fake_odom /twist_mrac_controller/odom"
 
 qos_overrides_file="qos_overrides.yaml"
 
-- 
2.12.0