Nav2 changes and improvements from SAR

src/Nav/aruco_localizer/config/aruco_board_params.yaml

@@ -12,7 +12,7 @@ aruco_board_detector_node:
     #          DICT_7X7_50, DICT_7X7_100, DICT_7X7_250, DICT_7X7_1000,
     #          DICT_ARUCO_ORIGINAL, DICT_APRILTAG_16h5, DICT_APRILTAG_25h9,
     #          DICT_APRILTAG_36h10, DICT_APRILTAG_36h11
-    aruco_dictionary_id: "DICT_6X6_250"
+    aruco_dictionary_id: "DICT_4X4_50"
 
     # Directory containing board configuration YAML files
     # This will be set by the launch file

src/Nav/gps/launch/rover.launch.py

@@ -3,7 +3,7 @@
 import launch
 import launch_ros.actions
 from ament_index_python.packages import get_package_share_directory
-from launch.actions import IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.actions import ExecuteProcess, DeclareLaunchArgument
 from launch.conditions import IfCondition

src/Nav/localization/config/ekf/gps_ekf_global.yaml

@@ -62,16 +62,16 @@ global_ekf:
     # odom0_pose_rejection_threshold: 1000000.0
     # odom0_twist_rejection_threshold: 1.0
 
-    odom1: /drive/odom
+    odom1: /chassis_controller/wheel_odom
     odom1_config: [
         # position
         false, false, false,
         # orientation
         false, false, false,
         # lineal vel
-        true, false, false,
+        true, true, false,
         # angular vel
-        false, false, false,
+        false, false, true,
         # lineal acc
         false, false, false,
       ]

src/Nav/localization/config/ekf/gps_ekf_local.yaml

@@ -62,16 +62,16 @@ local_ekf:
     # odom0_pose_rejection_threshold: 1000000.0
     # odom0_twist_rejection_threshold: 1.0
 
-    odom1: /drive/odom
+    odom1: /chassis_controller/wheel_odom
     odom1_config: [
         # position
         false, false, false,
         # orientation
         false, false, false,
         # lineal vel
-        true, false, false,
+        true, true, false,
         # angular vel
-        false, false, false,
+        false, false, true,
         # lineal acc
         false, false, false,
       ]

src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_global.yaml

@@ -4,7 +4,7 @@ global_ekf:
     # sensor_timeout: 20.0
     use_sim_time: false
 
-    two_d_mode: true
+    two_d_mode: false
 
     # transform_time_offset: 0.25
     # transform_timeout: 3.0
@@ -49,16 +49,36 @@ global_ekf:
     pose0_rejection_threshold: 50.0  # High threshold to accept bootstrap poses with high covariance
     # pose0_pose_rejection_threshold: 5.0  # Old parameter name (deprecated)
 
-    odom0: /drive/odom
+    pose1: /zed/zed_node/pose_with_covariance
+    pose1_config: [
+        # position
+        true, true, true,
+        # orientation
+        false, false, false,
+        # lineal vel 
+        false, false, false,
+        # angular vel
+        false, false, false,
+        # lineal acc
+        false, false, false,
+      ]
+    # pose1_queue_size: 2
+    # pose1_nodelay: false
+    pose1_differential: true
+    pose1_relative: false
+    pose1_pose_rejection_threshold: 5.0 # Mahalanobis Distance
+    # pose1_twist_rejection_threshold: 1.0
+
+    odom0: /chassis_controller/wheel_odom
     odom0_config: [
         # position
         false, false, false,
         # orientation
         false, false, false,
         # lineal vel
-        true, false, false,
+        true, true, false,
         # angular vel
-        false, false, false,
+        false, false, true,
         # lineal acc
         false, false, false,
       ]

src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_local.yaml

@@ -4,7 +4,7 @@ local_ekf:
     # sensor_timeout: 20.0
     use_sim_time: false
 
-    two_d_mode: true
+    two_d_mode: false
 
     # transform_time_offset: 0.25
     # transform_timeout: 3.0
@@ -49,16 +49,36 @@ local_ekf:
     pose0_rejection_threshold: 50.0  # High threshold to accept bootstrap poses with high covariance
     # pose0_pose_rejection_threshold: 5.0  # Old parameter name (deprecated)
 
-    odom0: /drive/odom
+    pose1: /zed/zed_node/pose_with_covariance
+    pose1_config: [
+        # position
+        true, true, false,
+        # orientation
+        false, false, false,
+        # lineal vel 
+        false, false, false,
+        # angular vel
+        false, false, false,
+        # lineal acc
+        false, false, false,
+      ]
+    # pose1_queue_size: 2
+    # pose1_nodelay: false
+    pose1_differential: true
+    pose1_relative: false
+    pose1_pose_rejection_threshold: 5.0 # Mahalanobis Distance
+    # pose1_twist_rejection_threshold: 1.0
+
+    odom0: /chassis_controller/wheel_odom
     odom0_config: [
         # position
         false, false, false,
         # orientation
         false, false, false,
         # lineal vel
-        true, false, false,
+        true, true, false,
         # angular vel
-        false, false, false,
+        false, false, true,
         # lineal acc
         false, false, false,
       ]

src/Nav/localization/localization/republish_odometry.py

@@ -76,7 +76,7 @@ def main(args=None):
     rclpy.init(args=args)
 
     # Set the frequency (in Hz) at which to republish the odometry
-    frequency = 10  # Change to your desired frequency (Hz)
+    frequency = 1.0  # Change to your desired frequency (Hz)
 
     # Create the node and start spinning
     node = OdometryRepublisher(frequency)