From 2dfc0513d66040015a65c22d7bc15894f243e279 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Tue, 20 Jan 2026 11:51:07 +0100
Subject: [PATCH 01/11] Added dual ukf for local and global localization

---
 .../config/example_params.yaml                |   2 +-
 .../config/example_params_summit_dual.yaml    | 200 ++++++++++++++++++
 .../FusionLocalizer.hpp                       |   3 +-
 .../easynav_fusion_localizer/ukf_wrapper.hpp  |   4 +-
 .../FusionLocalizer.cpp                       |  28 ++-
 .../easynav_fusion_localizer/ukf_wrapper.cpp  |  22 +-
 6 files changed, 241 insertions(+), 18 deletions(-)
 create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml

diff --git a/localizers/easynav_fusion_localizer/config/example_params.yaml b/localizers/easynav_fusion_localizer/config/example_params.yaml
index 9c47f30b..905be4e5 100644
--- a/localizers/easynav_fusion_localizer/config/example_params.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params.yaml
@@ -25,7 +25,7 @@ localizer_node:
       latitude_origin: 40.2834931
       longitude_origin: -3.8207253999999997
       altitude_origin: 746.184
-      local_filter:
+      global_filter:
         two_d_mode: true
         publish_tf: true
 
diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
new file mode 100644
index 00000000..3b0e1e43
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -0,0 +1,200 @@
+controller_node:
+  ros__parameters:
+    use_sim_time: true
+    controller_types: [dummy]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_controller/DummyController
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.0001
+
+localizer_node:
+  ros__parameters:
+    use_sim_time: true
+    localizer_types: [Ukf]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_localizer/DummyLocalizer
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.001
+    Ukf:
+      rt_freq: 50.0
+      freq: 5.0
+      reseed_freq: 0.1
+      plugin: easynav_fusion_localizer/FusionLocalizer
+      latitude_origin: 40.2834931
+      longitude_origin: -3.8207253999999997
+      altitude_origin: 746.184
+      local_filter:
+        two_d_mode: true
+        publish_tf: true
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: odom
+
+        # # --- INPUT 1: GPS Odometry ---
+        # gps0: /gps/fix
+        # gps0_config:
+        #   [true,  true,  false,
+        #   false, false, false,
+        #   false, false, false,
+        #   false, false, false,
+        #   false, false, false]
+        # gps0_differential: false
+
+        #--- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [true, true, true,
+          false, false, true,
+          true,  true, true,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        imu0: /imu/data
+        imu0_config:
+          [false, false, false,
+          true,  true,  true,
+          false, false, false,
+          true,  true,  true,
+          false, false, false]
+        imu0_differential: true
+        imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  false,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        process_noise_covariance: [0.05, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.05, 0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.06, 0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.06, 0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.025, 0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.025, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.04, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.01, 0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.01, 0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.02, 0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.01, 0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.01, 0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.015]
+      global_filter:
+        two_d_mode: true
+        publish_tf: true
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: map
+
+        # --- INPUT 1: GPS Odometry ---
+        gps0: /gps/fix
+        gps0_config:
+          [true,  true,  false,
+          false, false, false,
+          false, false, false,
+          false, false, false,
+          false, false, false]
+        gps0_differential: false
+
+        #--- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [false, false, false,
+          false, false, false,
+          true,  true, false,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        imu0: /imu/data
+        imu0_config:
+          [false, false, false,
+          true,  true,  true,
+          false, false, false,
+          true,  true,  true,
+          false, false, false]
+        imu0_differential: true
+        imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  false,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        process_noise_covariance: [0.05, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.05, 0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.06, 0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.06, 0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.025, 0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.025, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.04, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.01, 0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.01, 0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.02, 0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.01, 0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.01, 0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.015]
+
+
+
+maps_manager_node:
+  ros__parameters:
+    use_sim_time: true
+    map_types: [dummy]
+    dummy:
+      freq: 10.0
+      plugin: easynav_maps_manager/DummyMapsManager
+      cycle_time_nort: 0.1
+      cycle_time_rt: 0.0001
+
+planner_node:
+  ros__parameters:
+    use_sim_time: true
+    planner_types: [dummy]
+    dummy:
+      freq: 1.0
+      plugin: easynav_planner/DummyPlanner
+      cycle_time_nort: 0.2
+      cycle_time_rt: 0.0001
+
+sensors_node:
+  ros__parameters:
+    use_sim_time: true
+    forget_time: 0.5
+    sensors: [imu, gps]
+    perception_default_frame: odom
+    imu:
+      topic: /imu/data
+      type: sensor_msgs/msg/Imu
+    gps:
+      topic: /gps/fix
+      type: sensor_msgs/msg/NavSatFix
+
+system_node:
+  ros__parameters:
+    use_sim_time: true
+    position_tolerance: 0.1
+    angle_tolerance: 0.05
diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
index c29ce6e9..fcafdbb7 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
@@ -51,7 +51,8 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   void update(NavState & nav_state) override;
 
 private:
-  std::unique_ptr<robot_localization::UkfWrapper> ukf_wrapper_;
+  std::unique_ptr<robot_localization::UkfWrapper> ukf_local_{nullptr};
+  std::unique_ptr<robot_localization::UkfWrapper> ukf_global_{nullptr};
 
   int n_imu_sensors_{0};
   int n_gps_sensors_{0};
diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
index d9dbde9c..bdfecf3d 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
@@ -108,7 +108,8 @@ class UkfWrapper
   explicit UkfWrapper(
     std::shared_ptr<easynav::LocalizerNode> parent_node,
     const std::string & tf_prefix,
-    const std::string & plugin_name);
+    const std::string & plugin_name,
+    bool local_filter);
 
   //! @brief Destructor
   //!
@@ -870,6 +871,7 @@ class UkfWrapper
   std::string plugin_name_;
   std::string tf_prefix_;
   std::vector<CallbackData> gps_callbackData_arr_;
+  bool local_filter_;
 
 };
 
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index b212b73f..5d00624a 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -32,10 +32,14 @@ void FusionLocalizer::on_initialize()
     RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'",
     plugin_name.c_str());
 
-    ukf_wrapper_ = std::make_unique<robot_localization::UkfWrapper>(
-      localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter"
+    ukf_global_ = std::make_unique<robot_localization::UkfWrapper>(
+      localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false
     );
-    ukf_wrapper_->initialize();
+    ukf_local_ = std::make_unique<robot_localization::UkfWrapper>(
+      localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true
+    );
+    ukf_global_->initialize();
+    ukf_local_->initialize();
     localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0));
     localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_);
 
@@ -61,7 +65,7 @@ void FusionLocalizer::on_initialize()
   UTM_zone_ = std::to_string(zone) + (northp ? "N" : "S");
   UTM_origin_z_ = altitude_origin_;
 
-  n_gps_sensors_ = static_cast<int>(ukf_wrapper_->getGpsCallbackDataArr().size());
+  n_gps_sensors_ = static_cast<int>(ukf_global_->getGpsCallbackDataArr().size());
 
   RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully.");
 }
@@ -81,9 +85,9 @@ void FusionLocalizer::update_rt(NavState & nav_state)
         // nav_state.set("UTM_gnss_pose", pose);
         // Call the wrapper callback
         const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
-        ukf_wrapper_->poseCallback(
+        ukf_global_->poseCallback(
           std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(pose),
-          ukf_wrapper_->getGpsCallbackDataArr()[i], // callback_data
+          ukf_global_->getGpsCallbackDataArr()[i], // callback_data
           tf_info.map_frame, // target_frame
           tf_info.odom_frame,  // pose_source_frame
           false                           // imu_data
@@ -92,11 +96,15 @@ void FusionLocalizer::update_rt(NavState & nav_state)
     }
   }
 
-  ukf_wrapper_->periodicUpdate();
+  ukf_global_->periodicUpdate();
+  ukf_local_->periodicUpdate();
 
-  nav_msgs::msg::Odometry current_odom;
-  if (ukf_wrapper_->getFilteredOdometryMessage(&current_odom)) {
-    nav_state.set("robot_pose", current_odom);
+  nav_msgs::msg::Odometry global_odom, local_odom;
+  if (ukf_global_->getFilteredOdometryMessage(&global_odom)) {
+    nav_state.set("robot_pose", global_odom);
+  }
+  if (ukf_local_->getFilteredOdometryMessage(&local_odom)) {
+    nav_state.set("robot_pose_local", local_odom);
   }
 }
 
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
index 8bf14a01..71a065f0 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
@@ -86,7 +86,8 @@ using namespace std::chrono_literals;
 UkfWrapper::UkfWrapper(
   std::shared_ptr<easynav::LocalizerNode> parent_node,
   const std::string & tf_prefix,
-  const std::string & plugin_name)
+  const std::string & plugin_name,
+  bool local_filter)
 : print_diagnostics_(true),
   publish_acceleration_(false),
   publish_transform_(true),
@@ -114,7 +115,8 @@ UkfWrapper::UkfWrapper(
   last_set_pose_time_(0, 0, RCL_ROS_TIME),
   latest_control_time_(0, 0, RCL_ROS_TIME),
   tf_timeout_(0ns),
-  tf_time_offset_(0ns)
+  tf_time_offset_(0ns),
+  local_filter_(local_filter)
 {
   parent_node_ = parent_node;
   tf_prefix_ = tf_prefix;
@@ -895,7 +897,11 @@ void UkfWrapper::loadParams()
   odom_frame_id_ = tf_info.odom_frame;
   base_link_frame_id_ = tf_info.robot_footprint_frame;
   // World frame comes from Easynav TFInfo configuration
-  world_frame_id_ = tf_info.map_frame;
+  if(local_filter_) {
+    world_frame_id_ = tf_info.odom_frame;
+  } else {
+    world_frame_id_ = tf_info.map_frame;
+  }
 
   base_link_output_frame_id_ = base_link_frame_id_;
 
@@ -2262,15 +2268,21 @@ void UkfWrapper::initialize()
   // Position publisher
   rclcpp::PublisherOptions publisher_options;
   publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies();
+  std::string mode_prefix;
+  if (local_filter_) {
+    mode_prefix = "local_";
+  } else {
+    mode_prefix = "global_";
+  }
   position_pub_ =
     parent_node_->create_publisher<nav_msgs::msg::Odometry>(
-    "odometry/filtered", rclcpp::QoS(10), publisher_options);
+    mode_prefix + "odometry/filtered", rclcpp::QoS(10), publisher_options);
 
   // Optional acceleration publisher
   if (publish_acceleration_) {
     accel_pub_ =
       parent_node_->create_publisher<geometry_msgs::msg::AccelWithCovarianceStamped>(
-      "accel/filtered", rclcpp::QoS(10), publisher_options);
+      mode_prefix + "accel/filtered", rclcpp::QoS(10), publisher_options);
   }
 
   // Block commented as it is handled in the update_rt

From 1f5ca1bfcfbc95115833591cb9bbc88e1ae5d365 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Tue, 10 Feb 2026 14:56:46 +0100
Subject: [PATCH 02/11] Several bugs fixed. Now it correctly integrates GPS
 measurements to the dual filter.

---
 .../config/example_params_summit_dual.yaml    |  37 ++---
 .../FusionLocalizer.hpp                       |  18 ++-
 .../easynav_fusion_localizer/ukf_wrapper.hpp  |  11 +-
 .../FusionLocalizer.cpp                       | 146 +++++++++++++-----
 .../easynav_fusion_localizer/ukf_wrapper.cpp  |  41 +++--
 5 files changed, 164 insertions(+), 89 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
index 3b0e1e43..fef82461 100644
--- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -35,22 +35,12 @@ localizer_node:
         base_link_frame: base_link
         world_frame: odom
 
-        # # --- INPUT 1: GPS Odometry ---
-        # gps0: /gps/fix
-        # gps0_config:
-        #   [true,  true,  false,
-        #   false, false, false,
-        #   false, false, false,
-        #   false, false, false,
-        #   false, false, false]
-        # gps0_differential: false
-
         #--- INPUT 2: Wheel Odometry ---
-        odom0: /robotnik_base_control/odom
+        odom0: /robotnik_base_control/odom_fix
         odom0_config:
-          [true, true, true,
+          [true,  true, false,
           false, false, true,
-          true,  true, true,
+          true,  true, false,
           false, false, true,
           false, false, false]
         odom0_differential: false
@@ -59,11 +49,11 @@ localizer_node:
         imu0: /imu/data
         imu0_config:
           [false, false, false,
-          true,  true,  true,
           false, false, false,
           true,  true,  true,
+          true,  true,  true,
           false, false, false]
-        imu0_differential: true
+        imu0_differential: false
         imu0_remove_gravitational_acceleration: true
 
         # twist0: /gps/fix_velocity
@@ -105,18 +95,18 @@ localizer_node:
         gps0: /gps/fix
         gps0_config:
           [true,  true,  false,
-          false, false, false,
+          false, false, true,
           false, false, false,
           false, false, false,
           false, false, false]
         gps0_differential: false
 
-        #--- INPUT 2: Wheel Odometry ---
-        odom0: /robotnik_base_control/odom
+        # --- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom_fix
         odom0_config:
           [false, false, false,
           false, false, false,
-          true,  true, false,
+          true,  true,  false,
           false, false, true,
           false, false, false]
         odom0_differential: false
@@ -125,11 +115,11 @@ localizer_node:
         imu0: /imu/data
         imu0_config:
           [false, false, false,
-          true,  true,  true,
           false, false, false,
           true,  true,  true,
+          true,  true,  true,
           false, false, false]
-        imu0_differential: true
+        imu0_differential: false
         imu0_remove_gravitational_acceleration: true
 
         # twist0: /gps/fix_velocity
@@ -158,6 +148,11 @@ localizer_node:
                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.01, 0.0,
                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.015]
 
+        initial_estimate_covariance: [
+          5.0, 5.0, 1e-3,   1e-3, 1e-3, 1.0,
+          0.5,  0.5,  1e-3,   1e-3, 1e-3, 0.1,
+          0.1,  0.1,  0.1
+        ]
 
 
 maps_manager_node:
diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
index fcafdbb7..330869b2 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
@@ -1,17 +1,21 @@
 #pragma once
 
 #include <memory>
+#include <vector>
 
 #include "easynav_core/LocalizerMethodBase.hpp"
-#include "easynav_fusion_localizer/ukf_wrapper.hpp" // Tu wrapper refactorizado
+#include "easynav_fusion_localizer/ukf_wrapper.hpp"
 
 #include "sensor_msgs/msg/imu.hpp"
 #include "sensor_msgs/msg/nav_sat_fix.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 
 namespace easynav
 {
 
+constexpr double kNoOrientationCovariance = 1e5;
+
 /**
  * @class FusionLocalizer
  * @brief Plugin de localización para EasyNav que integra el UKF de
@@ -54,12 +58,14 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   std::unique_ptr<robot_localization::UkfWrapper> ukf_local_{nullptr};
   std::unique_ptr<robot_localization::UkfWrapper> ukf_global_{nullptr};
 
-  int n_imu_sensors_{0};
   int n_gps_sensors_{0};
 
   geometry_msgs::msg::PoseWithCovarianceStamped
   navsatfix_to_pose(const sensor_msgs::msg::NavSatFix & navsat_msg);
 
+  sensor_msgs::msg::NavSatFix
+  odom_to_navsatfix(const nav_msgs::msg::Odometry & odom_msg);
+
   double latitude_origin_{0.0};
   double longitude_origin_{0.0};
   double altitude_origin_{0.0};
@@ -67,8 +73,14 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   double UTM_origin_y_{0.0};
   double UTM_origin_z_{0.0};
   std::string UTM_zone_;
+  int UTM_zone_number_{0};
+  bool UTM_zone_northp_{true};
+
+  std::vector<rclcpp::Time> last_gps_stamp_;
 
-  std::vector<double> last_gps_stamp_;
+  rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsat_pub_{nullptr};
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr gps_debug_pub_{nullptr};
+  std::string navsatfix_topic_;
 
 };
 
diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
index bdfecf3d..1c7f4d35 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/ukf_wrapper.hpp
@@ -64,7 +64,6 @@
 #include "tf2/LinearMath/Transform.hpp"
 #include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_broadcaster.hpp>
-#include <tf2_ros/transform_listener.hpp>
 
 namespace robot_localization
 {
@@ -345,7 +344,7 @@ class UkfWrapper
   //!
   bool validateFilterOutput(nav_msgs::msg::Odometry * message);
 
-  std::vector<CallbackData> getGpsCallbackDataArr() const
+  const std::vector<CallbackData> & getGpsCallbackDataArr() const
   {
     return gps_callbackData_arr_;
   }
@@ -821,13 +820,9 @@ class UkfWrapper
   //!
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_srv_;
 
-  //! @brief Transform buffer for managing coordinate transforms
+  //! @brief Transform buffer for managing coordinate transforms (shared RTTFBuffer singleton)
   //!
-  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-
-  //! @brief Transform listener for receiving transforms
-  //!
-  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
+  tf2_ros::Buffer * tf_buffer_{nullptr};
 
   //! @brief broadcaster of worldTransform tfs
   //!
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index 5d00624a..147ce87b 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -6,6 +6,7 @@
 
 #include "easynav_common/types/IMUPerception.hpp"
 #include "easynav_common/types/GNSSPerception.hpp"
+#include "sensor_msgs/msg/nav_sat_status.hpp"
 
 #include <GeographicLib/UTMUPS.hpp>
 
@@ -23,8 +24,6 @@ void FusionLocalizer::on_initialize()
 
     auto localizer_node = std::dynamic_pointer_cast<LocalizerNode>(node);
 
-    last_gps_stamp_.resize(10, 0.0);
-
     const std::string & plugin_name = this->get_plugin_name();
     const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
@@ -48,6 +47,14 @@ void FusionLocalizer::on_initialize()
 
     localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0));
     localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_);
+
+    localizer_node->declare_parameter(
+      plugin_name + ".navsatfix_topic", std::string("global/navsatfix"));
+    localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_);
+    navsat_pub_ = localizer_node->create_publisher<sensor_msgs::msg::NavSatFix>(
+      navsatfix_topic_, rclcpp::QoS(10));
+    gps_debug_pub_ = localizer_node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+      "gps_pose", rclcpp::QoS(10));
   } catch (const std::exception & e) {
     RCLCPP_FATAL(
       get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s",
@@ -57,40 +64,41 @@ void FusionLocalizer::on_initialize()
   }
 
 
-  int zone;
-  bool northp;
-
-  GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, zone, northp, UTM_origin_x_,
-      UTM_origin_y_);
-  UTM_zone_ = std::to_string(zone) + (northp ? "N" : "S");
+  GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_,
+      UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_);
+  UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S");
   UTM_origin_z_ = altitude_origin_;
 
   n_gps_sensors_ = static_cast<int>(ukf_global_->getGpsCallbackDataArr().size());
+  last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME));
 
   RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully.");
 }
 
-// 2. Hook de actualización RT (Tu "Timer" de alta frecuencia)
 void FusionLocalizer::update_rt(NavState & nav_state)
 {
-  if(n_gps_sensors_ && nav_state.has("gnss")) {
+  EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_update_rt");
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+
+  if (n_gps_sensors_ && nav_state.has("gnss")) {
     auto gps_data = nav_state.get<GNSSPerceptions>(std::string("gnss"));
+    const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr();
     for (int i = 0; i < n_gps_sensors_; ++i) {
-      double gps_time = gps_data[i]->data.header.stamp.sec +
-        gps_data[i]->data.header.stamp.nanosec * 1e-9;
+      if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) {
+        continue;
+      }
+      rclcpp::Time gps_time(gps_data[i]->data.header.stamp);
       if (gps_time > last_gps_stamp_[i]) {
-      // if(true) {
+        EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps");
         last_gps_stamp_[i] = gps_time;
         auto pose = navsatfix_to_pose(gps_data[i]->data);
-        // nav_state.set("UTM_gnss_pose", pose);
-        // Call the wrapper callback
-        const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
+        gps_debug_pub_->publish(pose);
         ukf_global_->poseCallback(
           std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(pose),
-          ukf_global_->getGpsCallbackDataArr()[i], // callback_data
-          tf_info.map_frame, // target_frame
-          tf_info.odom_frame,  // pose_source_frame
-          false                           // imu_data
+          gps_cb_arr[i],
+          tf_info.map_frame,
+          gps_data[i]->data.header.frame_id,
+          false
         );
       }
     }
@@ -102,32 +110,49 @@ void FusionLocalizer::update_rt(NavState & nav_state)
   nav_msgs::msg::Odometry global_odom, local_odom;
   if (ukf_global_->getFilteredOdometryMessage(&global_odom)) {
     nav_state.set("robot_pose", global_odom);
+    navsat_pub_->publish(odom_to_navsatfix(global_odom));
   }
   if (ukf_local_->getFilteredOdometryMessage(&local_odom)) {
     nav_state.set("robot_pose_local", local_odom);
   }
 }
 
-// 3. Hook de actualización no-RT (baja frecuencia)
 void FusionLocalizer::update([[maybe_unused]] NavState & nav_state)
 {
 
 }
 
+/**
+ * @brief Converts a NavSatFix message to a PoseWithCovarianceStamped in UTM coordinates.
+ *
+ * This function transforms the latitude, longitude, and altitude from a sensor_msgs::msg::NavSatFix
+ * message into UTM coordinates and populates a geometry_msgs::msg::PoseWithCovarianceStamped message.
+ * The output pose is expressed relative to a predefined UTM origin. The orientation is set to identity
+ * (no rotation), as orientation cannot be inferred from GPS data alone.
+ *
+ * Covariance handling:
+ * - If the NavSatFix message provides a diagonal covariance (COVARIANCE_TYPE_DIAGONAL_KNOWN),
+ *   the corresponding variances are copied into the pose covariance matrix.
+ * - If not, a default variance of 1.0 is used for the position, and a warning is issued.
+ *
+ * @param navsat_msg The input NavSatFix message containing latitude, longitude, altitude, and covariance.
+ * @return geometry_msgs::msg::PoseWithCovarianceStamped The resulting pose in UTM coordinates with covariance.
+ *
+ * @note
+ * The function uses RCLCPP_WARN_THROTTLE to log a warning if the covariance type is unknown or invalid.
+ * This throttled warning will only be emitted at most once every 5 seconds, preventing log flooding,
+ * unlike RCLCPP_WARN which would log a warning every time the condition is met.
+ */
 geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose(
   const sensor_msgs::msg::NavSatFix & navsat_msg)
 {
   geometry_msgs::msg::PoseWithCovarianceStamped pose_msg;
-  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
-  // 1. Establecer el Header
-  // Usamos el mismo timestamp que el mensaje original
-  // y el world_frame_id que el filtro UKF espera (p.ej., "map" u "odom")
   pose_msg.header = navsat_msg.header;
 
+  const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
   pose_msg.header.frame_id = tf_info.map_frame;
 
-  // 2. Convertir coordenadas (Lat, Lon) a UTM (x, y)
   double utm_x, utm_y;
   int zone;
   bool northp;
@@ -138,7 +163,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
     zone,
     northp,
     utm_x,
-    utm_y);
+    utm_y,
+    UTM_zone_number_);
 
   pose_msg.pose.pose.position.x = utm_x - UTM_origin_x_;
   pose_msg.pose.pose.position.y = utm_y - UTM_origin_y_;
@@ -151,23 +177,63 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
 
   pose_msg.pose.covariance.fill(0.0);
 
-  pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0];  // xx
-  pose_msg.pose.covariance[1] = navsat_msg.position_covariance[1];  // xy
-  pose_msg.pose.covariance[2] = navsat_msg.position_covariance[2];  // xz
+  if (navsat_msg.position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN)
+  {
+    pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0];
+    pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4];
+    pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8];
+
+    pose_msg.pose.covariance[21] = kNoOrientationCovariance;
+    pose_msg.pose.covariance[28] = kNoOrientationCovariance;
+    pose_msg.pose.covariance[35] = kNoOrientationCovariance;
+  } else {
+      // Fallback variances if GPS doesn't provide them
+      double default_var = 1.0; // 1 meter variance standard
+      pose_msg.pose.covariance[0] = default_var;
+      pose_msg.pose.covariance[7] = default_var;
+      pose_msg.pose.covariance[14] = default_var;
+
+      pose_msg.pose.covariance[21] = kNoOrientationCovariance;
+      pose_msg.pose.covariance[28] = kNoOrientationCovariance;
+      pose_msg.pose.covariance[35] = kNoOrientationCovariance;
+      
+      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, 
+        "NavSatFix covariance type unknown or invalid. Using default covariance.");
+  }
 
-  pose_msg.pose.covariance[6] = navsat_msg.position_covariance[3];  // yx
-  pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4];  // yy
-  pose_msg.pose.covariance[8] = navsat_msg.position_covariance[5];  // yz
+  return pose_msg;
+}
 
-  pose_msg.pose.covariance[12] = navsat_msg.position_covariance[6]; // zx
-  pose_msg.pose.covariance[13] = navsat_msg.position_covariance[7]; // zy
-  pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8]; // zz
+sensor_msgs::msg::NavSatFix FusionLocalizer::odom_to_navsatfix(
+  const nav_msgs::msg::Odometry & odom_msg)
+{
+  sensor_msgs::msg::NavSatFix navsat_msg;
 
-  pose_msg.pose.covariance[21] = 99999.0;
-  pose_msg.pose.covariance[28] = 99999.0;
-  pose_msg.pose.covariance[35] = 99999.0;
+  navsat_msg.header = odom_msg.header;
+  navsat_msg.header.frame_id = "gps_link";
+  navsat_msg.status.status = sensor_msgs::msg::NavSatStatus::STATUS_FIX;
+  navsat_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS;
 
-  return pose_msg;
+  const double utm_x = odom_msg.pose.pose.position.x + UTM_origin_x_;
+  const double utm_y = odom_msg.pose.pose.position.y + UTM_origin_y_;
+
+  double latitude = 0.0;
+  double longitude = 0.0;
+  GeographicLib::UTMUPS::Reverse(
+    UTM_zone_number_, UTM_zone_northp_, utm_x, utm_y, latitude, longitude);
+
+  navsat_msg.latitude = latitude;
+  navsat_msg.longitude = longitude;
+  navsat_msg.altitude = odom_msg.pose.pose.position.z + UTM_origin_z_;
+
+  navsat_msg.position_covariance[0] = odom_msg.pose.covariance[0];
+  navsat_msg.position_covariance[4] = odom_msg.pose.covariance[7];
+  navsat_msg.position_covariance[8] = odom_msg.pose.covariance[14];
+
+  navsat_msg.position_covariance_type =
+    sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
+
+  return navsat_msg;
 }
 
 }  // namespace easynav
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
index 71a065f0..20e6ef05 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
@@ -75,7 +75,6 @@
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_broadcaster.hpp>
-#include <tf2_ros/transform_listener.hpp>
 
 #include "easynav_common/RTTFBuffer.hpp"
 
@@ -122,8 +121,8 @@ UkfWrapper::UkfWrapper(
   tf_prefix_ = tf_prefix;
   plugin_name_ = plugin_name;
 
-  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(parent_node_->get_clock());
-  tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
+  // Use the shared RTTFBuffer singleton instead of creating a private tf2_ros::Buffer
+  tf_buffer_ = easynav::RTTFBuffer::getInstance();
 
   state_variable_names_.push_back("X");
   state_variable_names_.push_back("Y");
@@ -149,8 +148,6 @@ UkfWrapper::~UkfWrapper()
   set_pose_sub_.reset();
   control_sub_.reset();
   stamped_control_sub_.reset();
-  tf_listener_.reset();
-  tf_buffer_.reset();
   diagnostic_updater_.reset();
   world_transform_broadcaster_.reset();
   set_pose_service_.reset();
@@ -186,8 +183,7 @@ void UkfWrapper::reset()
 
   last_diff_time_ = parent_node_->now().seconds();
 
-  // clear tf buffer to avoid TF_OLD_DATA errors
-  tf_buffer_->clear();
+  // Note: tf_buffer_ is the shared RTTFBuffer singleton, do not clear it here
 
   // clear last message timestamp, so older messages will be accepted
   last_message_times_.clear();
@@ -372,7 +368,7 @@ void UkfWrapper::enqueueMeasurement(
   const std::vector<bool> & update_vector, const double mahalanobis_thresh,
   const rclcpp::Time & time)
 {
-  MeasurementPtr meas = MeasurementPtr(new Measurement());
+  MeasurementPtr meas = std::make_shared<Measurement>();
 
   meas->topic_name_ = topic_name;
   meas->measurement_ = measurement;
@@ -1178,30 +1174,39 @@ void UkfWrapper::loadParams()
       "\npermit_corrected_publication is " << permit_corrected_publication_ <<
       "\nprint_diagnostics is " << print_diagnostics_ << "\n");
 
+  // Determine mode prefix for service/topic names to avoid collisions
+  // between local and global filter instances
+  std::string mode_prefix;
+  if (local_filter_) {
+    mode_prefix = "local_";
+  } else {
+    mode_prefix = "global_";
+  }
+
   // Create a subscriber for manually setting/resetting pose
   set_pose_sub_ =
     parent_node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
-    "set_pose", rclcpp::QoS(1),
+    mode_prefix + "set_pose", rclcpp::QoS(1),
     std::bind(&UkfWrapper::setPoseCallback, this, std::placeholders::_1), options);
 
   // Create a service for manually setting/resetting pose
   set_pose_service_ =
     parent_node_->create_service<robot_localization::srv::SetPose>(
-    "set_pose", std::bind(
+    mode_prefix + "set_pose", std::bind(
       &UkfWrapper::setPoseSrvCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
   // Create a service for manually enabling the filter
   enable_filter_srv_ =
     parent_node_->create_service<std_srvs::srv::Empty>(
-    "~/enable", std::bind(
+    "~/" + mode_prefix + "enable", std::bind(
       &UkfWrapper::enableFilterSrvCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
   // Create a service for manually setting/resetting pose
   reset_srv_ =
     parent_node_->create_service<std_srvs::srv::Empty>(
-    "reset", std::bind(
+    mode_prefix + "reset", std::bind(
       &UkfWrapper::resetSrvCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
@@ -1209,7 +1214,7 @@ void UkfWrapper::loadParams()
   // publishing
   toggle_filter_processing_srv_ =
     parent_node_->create_service<robot_localization::srv::ToggleFilterProcessing>(
-    "~/toggle", std::bind(
+    "~/" + mode_prefix + "toggle", std::bind(
       &UkfWrapper::toggleFilterProcessingCallback, this,
       std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
 
@@ -2096,6 +2101,8 @@ void UkfWrapper::odometryCallback(
     pos_ptr->header = msg->header;
     pos_ptr->pose = msg->pose;  // Entire pose object, also copies covariance
 
+    
+
     if (pose_callback_data.pose_use_child_frame_) {
       poseCallback(pos_ptr, pose_callback_data, world_frame_id_, msg->child_frame_id, false);
     } else {
@@ -2876,7 +2883,7 @@ bool UkfWrapper::prepareAcceleration(
   // we have to handle the situation.
   tf2::Transform target_frame_trans;
   bool can_transform = ros_filter_utilities::lookupTransformSafe(
-    tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_,
+    tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_,
     target_frame_trans);
 
   if (can_transform) {
@@ -3105,7 +3112,7 @@ bool UkfWrapper::preparePose(
   // 2. Get the target frame transformation
   tf2::Transform target_frame_trans;
   bool can_transform = ros_filter_utilities::lookupTransformSafe(
-    tf_buffer_.get(), final_target_frame, pose_tmp.frame_id_,
+    tf_buffer_, final_target_frame, pose_tmp.frame_id_,
     rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_,
     target_frame_trans);
 
@@ -3115,7 +3122,7 @@ bool UkfWrapper::preparePose(
   bool can_src_transform = false;
   if (source_frame != base_link_frame_id_) {
     can_src_transform = ros_filter_utilities::lookupTransformSafe(
-      tf_buffer_.get(), source_frame, base_link_frame_id_,
+      tf_buffer_, source_frame, base_link_frame_id_,
       rclcpp::Time(tf2::timeToSec(pose_tmp.stamp_)), tf_timeout_,
       source_frame_trans);
   }
@@ -3537,7 +3544,7 @@ bool UkfWrapper::prepareTwist(
   // 4. We need to transform this into the target frame (probably base_link)
   tf2::Transform target_frame_trans;
   bool can_transform = ros_filter_utilities::lookupTransformSafe(
-    tf_buffer_.get(), target_frame, msg_frame, msg->header.stamp, tf_timeout_,
+    tf_buffer_, target_frame, msg_frame, msg->header.stamp, tf_timeout_,
     target_frame_trans);
 
   if (can_transform) {

From 767a07b4f81a177266c574b8fc6f061cae89c859 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Mon, 16 Feb 2026 11:36:31 +0100
Subject: [PATCH 03/11] fixed odom topic

---
 .../config/example_params_summit_dual.yaml                  | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
index fef82461..52e7b06e 100644
--- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -36,7 +36,7 @@ localizer_node:
         world_frame: odom
 
         #--- INPUT 2: Wheel Odometry ---
-        odom0: /robotnik_base_control/odom_fix
+        odom0: /robotnik_base_control/odom
         odom0_config:
           [true,  true, false,
           false, false, true,
@@ -102,7 +102,7 @@ localizer_node:
         gps0_differential: false
 
         # --- INPUT 2: Wheel Odometry ---
-        odom0: /robotnik_base_control/odom_fix
+        odom0: /robotnik_base_control/odom
         odom0_config:
           [false, false, false,
           false, false, false,
@@ -126,7 +126,7 @@ localizer_node:
         # twist0_config:
         #   [false, false, false,
         #   false, false, false,
-        #   true,  false,  false,
+        #   true,  true,  false,
         #   false, false, false,
         #   false, false, false]
         # twist0_differential: false

From d7cf00bbdb2da71f7f19211f45d1c1aa125729c4 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Mon, 23 Feb 2026 12:49:48 +0100
Subject: [PATCH 04/11] Call set pose srvc for first pose for fastest
 convergence. Updated params to stabilice the filter in summit

---
 .../config/example_params_summit_dual.yaml    | 31 ++++++++++++-------
 .../FusionLocalizer.hpp                       |  2 ++
 .../FusionLocalizer.cpp                       | 29 ++++++++++-------
 3 files changed, 39 insertions(+), 23 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
index 52e7b06e..229f09c2 100644
--- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -46,15 +46,15 @@ localizer_node:
         odom0_differential: false
 
         # --- INPUT 3: IMU ---
-        imu0: /imu/data
-        imu0_config:
-          [false, false, false,
-          false, false, false,
-          true,  true,  true,
-          true,  true,  true,
-          false, false, false]
-        imu0_differential: false
-        imu0_remove_gravitational_acceleration: true
+        # imu0: /imu/data
+        # imu0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   false, false, false,
+        #   false, false, true,
+        #   true, true, true]
+        # imu0_differential: false
+        # imu0_remove_gravitational_acceleration: true
 
         # twist0: /gps/fix_velocity
         # twist0_config:
@@ -84,6 +84,13 @@ localizer_node:
       global_filter:
         two_d_mode: true
         publish_tf: true
+        debug: true
+        debug_out_file: /tmp/ukf_global_debug.txt
+        transform_timeout: 0.1
+        smooth_lagged_data: true
+        # Defines the time window in seconds to keep past states in memory.
+        history_length: 5.0
+        dynamic_process_noise_covariance: true
 
         # --- Reference Frames ---
         map_frame: map
@@ -95,7 +102,7 @@ localizer_node:
         gps0: /gps/fix
         gps0_config:
           [true,  true,  false,
-          false, false, true,
+          false, false, false,
           false, false, false,
           false, false, false,
           false, false, false]
@@ -116,8 +123,8 @@ localizer_node:
         imu0_config:
           [false, false, false,
           false, false, false,
-          true,  true,  true,
-          true,  true,  true,
+          false, false, false,
+          false, false, true,
           false, false, false]
         imu0_differential: false
         imu0_remove_gravitational_acceleration: true
diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
index 330869b2..19f829b6 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
@@ -82,6 +82,8 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr gps_debug_pub_{nullptr};
   std::string navsatfix_topic_;
 
+  bool first_pose_received_{false};
+
 };
 
 }  // namespace easynav
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index 147ce87b..7fe7e41c 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -91,10 +91,16 @@ void FusionLocalizer::update_rt(NavState & nav_state)
       if (gps_time > last_gps_stamp_[i]) {
         EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps");
         last_gps_stamp_[i] = gps_time;
-        auto pose = navsatfix_to_pose(gps_data[i]->data);
-        gps_debug_pub_->publish(pose);
+        auto pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(navsatfix_to_pose(gps_data[i]->data));
+        if (!first_pose_received_) {
+          RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state.");
+          ukf_global_->setPoseCallback(pose);
+          first_pose_received_ = true;
+          continue;
+        }
+        gps_debug_pub_->publish(*pose);
         ukf_global_->poseCallback(
-          std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(pose),
+          pose,
           gps_cb_arr[i],
           tf_info.map_frame,
           gps_data[i]->data.header.frame_id,
@@ -176,6 +182,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
   pose_msg.pose.pose.orientation.w = 1.0;
 
   pose_msg.pose.covariance.fill(0.0);
+  
+  double default_var = 1.0; // 1 meter variance standard
 
   if (navsat_msg.position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN)
   {
@@ -183,20 +191,19 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
     pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4];
     pose_msg.pose.covariance[14] = navsat_msg.position_covariance[8];
 
-    pose_msg.pose.covariance[21] = kNoOrientationCovariance;
-    pose_msg.pose.covariance[28] = kNoOrientationCovariance;
-    pose_msg.pose.covariance[35] = kNoOrientationCovariance;
+    pose_msg.pose.covariance[21] = default_var;
+    pose_msg.pose.covariance[28] = default_var;
+    pose_msg.pose.covariance[35] = default_var;
   } else {
       // Fallback variances if GPS doesn't provide them
-      double default_var = 1.0; // 1 meter variance standard
       pose_msg.pose.covariance[0] = default_var;
       pose_msg.pose.covariance[7] = default_var;
       pose_msg.pose.covariance[14] = default_var;
 
-      pose_msg.pose.covariance[21] = kNoOrientationCovariance;
-      pose_msg.pose.covariance[28] = kNoOrientationCovariance;
-      pose_msg.pose.covariance[35] = kNoOrientationCovariance;
-      
+      pose_msg.pose.covariance[21] = default_var;
+      pose_msg.pose.covariance[28] = default_var;
+      pose_msg.pose.covariance[35] = default_var;
+
       RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, 
         "NavSatFix covariance type unknown or invalid. Using default covariance.");
   }

From 8b2d7f7ce7bae062a12ac8c518a7f17009cc03ec Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Mon, 23 Feb 2026 15:36:15 +0100
Subject: [PATCH 05/11] Uncrustify changes

---
 .../FusionLocalizer.cpp                       | 29 +++++++++++--------
 .../easynav_fusion_localizer/ukf_wrapper.cpp  |  1 -
 2 files changed, 17 insertions(+), 13 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index 7fe7e41c..99605305 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -53,7 +53,8 @@ void FusionLocalizer::on_initialize()
     localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_);
     navsat_pub_ = localizer_node->create_publisher<sensor_msgs::msg::NavSatFix>(
       navsatfix_topic_, rclcpp::QoS(10));
-    gps_debug_pub_ = localizer_node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+    gps_debug_pub_ =
+      localizer_node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
       "gps_pose", rclcpp::QoS(10));
   } catch (const std::exception & e) {
     RCLCPP_FATAL(
@@ -91,9 +92,12 @@ void FusionLocalizer::update_rt(NavState & nav_state)
       if (gps_time > last_gps_stamp_[i]) {
         EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps");
         last_gps_stamp_[i] = gps_time;
-        auto pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(navsatfix_to_pose(gps_data[i]->data));
+        auto pose =
+          std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(navsatfix_to_pose(
+            gps_data[i]->data));
         if (!first_pose_received_) {
-          RCLCPP_INFO(get_node()->get_logger(), "First valid GPS fix received. Initializing filter state.");
+          RCLCPP_INFO(get_node()->get_logger(),
+              "First valid GPS fix received. Initializing filter state.");
           ukf_global_->setPoseCallback(pose);
           first_pose_received_ = true;
           continue;
@@ -182,10 +186,11 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
   pose_msg.pose.pose.orientation.w = 1.0;
 
   pose_msg.pose.covariance.fill(0.0);
-  
+
   double default_var = 1.0; // 1 meter variance standard
 
-  if (navsat_msg.position_covariance_type == sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN)
+  if (navsat_msg.position_covariance_type ==
+    sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN)
   {
     pose_msg.pose.covariance[0] = navsat_msg.position_covariance[0];
     pose_msg.pose.covariance[7] = navsat_msg.position_covariance[4];
@@ -196,15 +201,15 @@ geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose
     pose_msg.pose.covariance[35] = default_var;
   } else {
       // Fallback variances if GPS doesn't provide them
-      pose_msg.pose.covariance[0] = default_var;
-      pose_msg.pose.covariance[7] = default_var;
-      pose_msg.pose.covariance[14] = default_var;
+    pose_msg.pose.covariance[0] = default_var;
+    pose_msg.pose.covariance[7] = default_var;
+    pose_msg.pose.covariance[14] = default_var;
 
-      pose_msg.pose.covariance[21] = default_var;
-      pose_msg.pose.covariance[28] = default_var;
-      pose_msg.pose.covariance[35] = default_var;
+    pose_msg.pose.covariance[21] = default_var;
+    pose_msg.pose.covariance[28] = default_var;
+    pose_msg.pose.covariance[35] = default_var;
 
-      RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000, 
+    RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 5000,
         "NavSatFix covariance type unknown or invalid. Using default covariance.");
   }
 
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
index 20e6ef05..6164b4de 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp
@@ -2101,7 +2101,6 @@ void UkfWrapper::odometryCallback(
     pos_ptr->header = msg->header;
     pos_ptr->pose = msg->pose;  // Entire pose object, also copies covariance
 
-    
 
     if (pose_callback_data.pose_use_child_frame_) {
       poseCallback(pos_ptr, pose_callback_data, world_frame_id_, msg->child_frame_id, false);

From 350bb8bc5d551ab471ddacc4bc9081fd570e0d82 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Tue, 24 Feb 2026 09:27:58 +0100
Subject: [PATCH 06/11] Initialization considers IMU orientation

---
 .../config/example_params_summit_dual.yaml    |  2 +-
 .../FusionLocalizer.cpp                       | 28 ++++---------------
 2 files changed, 7 insertions(+), 23 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
index 229f09c2..3e5d143c 100644
--- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -89,7 +89,7 @@ localizer_node:
         transform_timeout: 0.1
         smooth_lagged_data: true
         # Defines the time window in seconds to keep past states in memory.
-        history_length: 5.0
+        # history_length: 5.0
         dynamic_process_noise_covariance: true
 
         # --- Reference Frames ---
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index 99605305..825c6a09 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -78,7 +78,6 @@ void FusionLocalizer::on_initialize()
 
 void FusionLocalizer::update_rt(NavState & nav_state)
 {
-  EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_update_rt");
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
   if (n_gps_sensors_ && nav_state.has("gnss")) {
@@ -98,6 +97,12 @@ void FusionLocalizer::update_rt(NavState & nav_state)
         if (!first_pose_received_) {
           RCLCPP_INFO(get_node()->get_logger(),
               "First valid GPS fix received. Initializing filter state.");
+          if(nav_state.has("imu")) {
+            auto imu_data = nav_state.get<IMUPerceptions>(std::string("imu"));
+            if (!imu_data.empty()) {
+              pose->pose.pose.orientation = imu_data[0]->data.orientation;
+            }
+          }
           ukf_global_->setPoseCallback(pose);
           first_pose_received_ = true;
           continue;
@@ -132,27 +137,6 @@ void FusionLocalizer::update([[maybe_unused]] NavState & nav_state)
 
 }
 
-/**
- * @brief Converts a NavSatFix message to a PoseWithCovarianceStamped in UTM coordinates.
- *
- * This function transforms the latitude, longitude, and altitude from a sensor_msgs::msg::NavSatFix
- * message into UTM coordinates and populates a geometry_msgs::msg::PoseWithCovarianceStamped message.
- * The output pose is expressed relative to a predefined UTM origin. The orientation is set to identity
- * (no rotation), as orientation cannot be inferred from GPS data alone.
- *
- * Covariance handling:
- * - If the NavSatFix message provides a diagonal covariance (COVARIANCE_TYPE_DIAGONAL_KNOWN),
- *   the corresponding variances are copied into the pose covariance matrix.
- * - If not, a default variance of 1.0 is used for the position, and a warning is issued.
- *
- * @param navsat_msg The input NavSatFix message containing latitude, longitude, altitude, and covariance.
- * @return geometry_msgs::msg::PoseWithCovarianceStamped The resulting pose in UTM coordinates with covariance.
- *
- * @note
- * The function uses RCLCPP_WARN_THROTTLE to log a warning if the covariance type is unknown or invalid.
- * This throttled warning will only be emitted at most once every 5 seconds, preventing log flooding,
- * unlike RCLCPP_WARN which would log a warning every time the condition is met.
- */
 geometry_msgs::msg::PoseWithCovarianceStamped FusionLocalizer::navsatfix_to_pose(
   const sensor_msgs::msg::NavSatFix & navsat_msg)
 {

From 34aef9fcb1ccf706550b9ac5ac5daf91e940bd78 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Wed, 25 Feb 2026 17:26:22 +0100
Subject: [PATCH 07/11] Added simple configuration to launch full demo

---
 .../easynav_fusion_localizer/CMakeLists.txt   |   4 ++
 .../config/example_params_summit_dual.yaml    |  56 ++++++++++++++++--
 .../easynav_fusion_localizer/maps/empty.png   | Bin 0 -> 4849 bytes
 .../easynav_fusion_localizer/maps/empty.yaml  |   7 +++
 .../easynav_fusion_localizer/maps/square.png  | Bin 0 -> 11002 bytes
 .../easynav_fusion_localizer/maps/square.yaml |   7 +++
 6 files changed, 70 insertions(+), 4 deletions(-)
 create mode 100644 localizers/easynav_fusion_localizer/maps/empty.png
 create mode 100644 localizers/easynav_fusion_localizer/maps/empty.yaml
 create mode 100644 localizers/easynav_fusion_localizer/maps/square.png
 create mode 100644 localizers/easynav_fusion_localizer/maps/square.yaml

diff --git a/localizers/easynav_fusion_localizer/CMakeLists.txt b/localizers/easynav_fusion_localizer/CMakeLists.txt
index 4db3bed8..94db9550 100644
--- a/localizers/easynav_fusion_localizer/CMakeLists.txt
+++ b/localizers/easynav_fusion_localizer/CMakeLists.txt
@@ -72,6 +72,10 @@ install(TARGETS
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
+install(DIRECTORY maps/
+  DESTINATION share/${PROJECT_NAME}/maps
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)
diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
index 3e5d143c..482386e8 100644
--- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -1,12 +1,25 @@
 controller_node:
   ros__parameters:
     use_sim_time: true
-    controller_types: [dummy]
+    controller_types: [mppi]
     dummy:
       rt_freq: 30.0
       plugin: easynav_controller/DummyController
       cycle_time_nort: 0.01
       cycle_time_rt: 0.0001
+    mppi:
+      rt_freq: 30.0 
+      plugin: easynav_mppi_controller/MPPIController
+      num_samples: 200
+      horizon_steps: 5
+      dt: 0.15
+      lambda: 1.0      
+      fov: 1.57
+      max_linear_velocity: 1.5
+      max_angular_velocity: 1.0
+      max_linear_acceleration: 0.5
+      max_angular_acceleration: 0.5
+      safety_radius: 0.3
 
 localizer_node:
   ros__parameters:
@@ -165,7 +178,32 @@ localizer_node:
 maps_manager_node:
   ros__parameters:
     use_sim_time: true
-    map_types: [dummy]
+    map_types: [costmap]
+    routes:
+      freq: 10.0
+      plugin: easynav_routes_maps_manager/RoutesMapsManager
+      filters: [routes_costmap]
+      package: easynav_fusion_localizer
+      map_path_file: routes/simple_square.yaml
+      routes_costmap:
+        plugin: easynav_routes_maps_manager/RoutesCostmapFilter
+        min_cost: 50
+        route_width: 0.5
+      cycle_time_nort: 0.001
+      cycle_time_rt: 0.001
+    costmap:
+      freq: 10.0
+      plugin: easynav_costmap_maps_manager/CostmapMapsManager
+      package: easynav_fusion_localizer
+      map_path_file: maps/square.yaml
+      filters: [obstacles, inflation]
+      obstacles:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter
+      inflation:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter
+        inflation_radius: 1.3
+        inscribed_radius: 0.25
+        cost_scaling_factor: 3.0
     dummy:
       freq: 10.0
       plugin: easynav_maps_manager/DummyMapsManager
@@ -175,18 +213,23 @@ maps_manager_node:
 planner_node:
   ros__parameters:
     use_sim_time: true
-    planner_types: [dummy]
+    planner_types: [simple]
     dummy:
       freq: 1.0
       plugin: easynav_planner/DummyPlanner
       cycle_time_nort: 0.2
       cycle_time_rt: 0.0001
+    simple:
+      freq: 10.0
+      plugin: easynav_costmap_planner/CostmapPlanner
+      cost_factor: 10.0
+      continuous_replan: true
 
 sensors_node:
   ros__parameters:
     use_sim_time: true
     forget_time: 0.5
-    sensors: [imu, gps]
+    sensors: [imu, gps, laser1]
     perception_default_frame: odom
     imu:
       topic: /imu/data
@@ -194,6 +237,11 @@ sensors_node:
     gps:
       topic: /gps/fix
       type: sensor_msgs/msg/NavSatFix
+    laser1:
+      topic: /front_laser/points
+      type: sensor_msgs/msg/PointCloud2
+      group: points
+    
 
 system_node:
   ros__parameters:
diff --git a/localizers/easynav_fusion_localizer/maps/empty.png b/localizers/easynav_fusion_localizer/maps/empty.png
new file mode 100644
index 0000000000000000000000000000000000000000..cd6eb69a9179a64f6596a543a2ed9dbc40bb35fc
GIT binary patch
literal 4849
zcmeAS@N?(olHy`uVBq!ia0y~yV15C@985rwLkFEVGcYi=WI8(scse^P6cpvBW#*(Z
zFlbDyow(oou!GFe_+ZzGT}`rG7AY(I$^s|$ZRK)ZA&}J?ChMd3m(^Ell8Ctb!L1J-
z^dDW-+`V}n-?}D+AIwMW9p;F{E;;gEwN=bePUic=yL*q{wRXq|o0;}ig~50GSsyLU
zXP;Il-PdMuG3c>inHhO(j>^)9*8AQGK6oA5lm2u5?_!=+tlf-^6D<z%TvofMFz@ab
z-gCc?B!*23eN_C&bF!YtpA)4=ADf?3oz6N>(rok0%0~tpBVHvf?T);8r6av#@v3FP
z`zCyjc)I6k<mzjYzvumFUD&j5)0xs)$sUP<-V+p@9A!mA5|!On`*pSQyj8FLvv^&+
z(sZYFToRUI0nKbTzc&9r_&#s#qc2mYi+C~Z)$jXk$oJ(f)7rMjHS*SeuUR<kn8T}%
z$7eHr^uHu6t6RmwlgliAUDo#|vtD__iOGtyH)*qGavfFrQ0I1k3cFigW25Q%?cePD
z<QsHKr|x6q<7?~`N?reZ&+h%Zx7VNF^7}XAoV!KabMj{cV}*gUz$3Dlfk96hgc&QA
z+Lr+Z*-JcqUD;nTNeEa-i|)!W1`0`*xJHyX=jZ08=9Mrw7o{eaq^2m8XO?6rxO@5r
zgg5euGcX9;_H=O!skrs_njs?t1IJ;51N)yRHcnQacwzP2B@7G=_uYUA!QdT`Rmjf3
z@R*N*L86?2;lK`Nh(M1u14F_cMz8?GF<Ax%m;hKQNC3ZHm|?i}f=q|o3pWg84OYD%
zmw^ofDaEdruqROTQo|D{fkzcjAO`^0bXdS+4*<fRATbsRdjcf|V2=qBJpqbvxV?m9
z5f&mS4nc_tlw^!GIict!><N@)j6Ehu^u#bsim{_53ZZmNI4z>2V|b=O&QPSLMP$9B
zB?_pvgB2j~j0_V%DHTv+VzfkoH5HJP6RnftXo*6Fh89XX9xYL@rDGzBMG~v`(GmsP
zfxum)4oFfQEm1~G6fBK2!f6pD9TV;_jFu=^8(N@nh6f(uzAM(|a>H!ah6Jv&ReOqb
PL4ziqu6{1-oD!M<xv|h9

literal 0
HcmV?d00001

diff --git a/localizers/easynav_fusion_localizer/maps/empty.yaml b/localizers/easynav_fusion_localizer/maps/empty.yaml
new file mode 100644
index 00000000..629ca4cb
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/maps/empty.yaml
@@ -0,0 +1,7 @@
+image: empty.png
+mode: trinary
+resolution: 0.2
+origin: [-100.0, -100.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
diff --git a/localizers/easynav_fusion_localizer/maps/square.png b/localizers/easynav_fusion_localizer/maps/square.png
new file mode 100644
index 0000000000000000000000000000000000000000..2e6d85275d3d37d8ebd8bca680f043604c99d85c
GIT binary patch
literal 11002
zcmeHMc~lc=5^s({MFm+eK-4yjD~u-T9ORNi1>`cqa44*@+DSSXO-M)r2{>TH=RKZ-
zI)W&x!YB&rIEn`fjv_h^qRZ~W0BXEPxuT2;D!5;F5``Uo`y6%u+3+5rySl2rs$c!;
ztFOb#i0MHC?Op952pSj?91saXHoxM(eeHmRzv~}&z#l_ebgViO)xZj+Tr81dusT(N
z!I)Meh9GVInlERqwH?#<<+C9wTiZbH+OBU!@`IH(B9qe6HijJi(>xD}2W?xKU$x;^
z{ZC`=M*cIpU-0IyM`VT$ZZK^4yTK-5Y|_*7%WJKcJiPkzg}R(vy;D$i)$(&OWv)dq
zE1;7%ud1`<vi$n`rUdtyaMJXGvx^K<e=PQ>`brZx|43tTO0zoX)PxYU$~7!Ad|LjV
zF~!sN&fO6-{CVRqQ+Ig}e@>clI@Be*Bg?~<x-%+Y9VNNlFqfN$gig+k)NVMg{V{5a
zbykk!<9$!D({1<auMU5C|1UQkonkfPm~k1WAjoQ|#NR(6#NXeXJIHuj`i8Qwvy<E^
z=0xR3j<-*VNV*V_+q{#s_K1Dpo<Z}M&KNn3()AhNb>ooXL1&KW_3J7Mm#jWq>`<Hu
zci5cso3w36{O)sYW0wv)G|KJS&1<JBuT7k^XUkS`as|XMsfs31#NY1|=!aS_qv_x@
zOzfiCm=#d@DA{rJoVK0b``VBD1?|W_?5rQWQus3>yEX9ER(^xNG~7)b7&q2;+;-$a
z=DBsVXL@Jco~?+}Z$%lo+2b4ruU^WUQUC2w{u*sIXXkw0-Tp4?VlL0xc0{?!tL@?(
z=bs*ozcl+h?kd}1hiv<ihD3ik#f7!=hljgkgRf?kE?IQ$N!Xpp3o+L2mDXQeflp=_
z{5q}`ZQ7Cj+r}wLc`quGe9!C3DOdJzFF!kUn>ihN#Zcszyxhb?UWB9-<p-|Sb<6a(
zPUL$oEWhi3{3g0k&>WhPJ;m3tbjMXTw)o5}_}INC|7xs!V6(oUcq6;~%oyaadCgB8
z+FB)TEjJid-5Jo7cI7vzS6602kfW~zkTI4&jVF}LD5yv-z$jXo0<aW<y#2HaRJZ_B
z!vZWplITNfJ6T48B_bcvOfNpdSNLP|B*AG)EGlh!v@mUfkSikj`PzGHdBA}TQ=_m}
zCQVfFv_2%lFArSfVk!wHRMZQ6NU{6~*k7*1U?zo0LCAqxNeZ3hYY%%XMPgoLz*Lh9
zaOXpsr&cR?RH{a!p=cNsxiW!D<8rxFgifW?$v}gwN=;OwT5_Vw9d}{$5rC<LN{K=(
zktf2qPgEdJR{M}h;5lrLPp07WU%O9KnNR?DP_?LnN~0iDnT*=gLZuE&0ZvQ-z12b$
z4SJdyiK*ntN+A}Qf+ecmd%6$_U)w8^l~N)dk&uc>F&R)*fmLa}mJABvN4&PcAxMzO
z6oeH3yO*X~B7Or_uh{S>L^?eK0p_p$_R==zPACH{KA#sL7bfH3g#`GJ@b!5jxlkhF
z5tkeRmrLWaxMVidOF(8K7>z8TalFVZwur$6)IeAqMh{mZi7GXkD8z7Az&S+%{9ppM
z2oo~dWHt+YFcATrEMU_ZWR6(CMQL;qSIG40;UZiq0jxx&J)^>1iGVAH0Oc^nVg{MU
zpb5!L4qym}<HaH)47!L#<FLhaA(wC^67r_Xl`<5RQzAnXFsdRkfp~xe&YK(&;zOcS
zkT;SDDXJC&10T{fNn*10&4Xx(42x2uIGQvzn~pHNI1GeCqcIU<+JwIum{J9pi2I}=
z6b6eh$IHS4$pBzcoTtD6AqTne{FNB0mMf#>a;XmqcLC#;uOIV4JBd&=8i1-X;1r>=
zcnF<G_ll;mc{GH_K*l399@0Z!E|Q2-|C=`6Jg~R1<iQdZSU;5z89OQpOETUWZ>18V
zm0*}?3LYvnrl3MoFcA?a@MU}?oQEbRU|{z!(bX)MyoCxhp@7N8ONW9|k(nYkMiu}O
zS;Xc7sesEwX=rcnD!Ev#L6z9#1b`#J3ec0l3icoh<=I<XGY`Y7001K+bTSSYi^ufh
zAzTw+)K8xxhO&h$E{8#;Gesy!Q6M67Xmpe;#5fpD#1zmF)X4VN0DfqSY(NDbjrqSz
z5oNOIUSbR(bC?*YqL?cta}ZQSrg4O9j42ihx$KXgVo%zes_0F{cYZT(y{W%rwo#$S
zr~uVB%D@N$6OH<Mq?u>|O7baxOda(pmw;i*BJYK-1uhF*?}fm7DO=cOf$O~xcrRrO
zyFMCR_HSN_u|#l|)qq#tddD*q@Vaa#2nz}zUYwy$|C5WrRX;^=oC<;{?)a~jHs3D=
z2>Yl*_<?;MTDuIgABDIb*aJi^>cCjFzg&jDTS3H;9}~eEiFzK4ANgl4b{Gyp&Z|QL
zCP!=QTdvVoHP*Q<yZLJN0K1@~;6tpu@+L#3{Wf;}x{;|a_T!y4g={EvU+j71b5+$+
zmpS1JqtkasRS(WCc+kH0Z(4Fp@~RbHU`^frZlN{!I|zI|1?eXB0iTN<z(-%TMff3r
zOv<w{o7?itcv^%6!TvBHko8CaC1DO<v4S81PmAya0U=tSz|+|J1jrU4PO$d|<eDuo
zvS3M!kdMYA%UJppBl!>S!p3q~%<&V_e@^uO)3LPb*=1Yk<$>gQP5a?#HAB*GRC-tY
zb!DT@x757j3aI>M{)*TtsC<-7dM!22)5Qv^%L;0I`vC7)UEK6Fv4>tLlDL8+^Se)?
z`rLj{$AQ8^R#%2+ep%PE;->CL<Ic~F87ga-8edBp1V#KX#A%-kv|@#GWm3(c?zp_R
zitoaA+`6=d8n}HR#2K~jn2q7=u50<-&z~&Vopr1Iel^7xTAw@dSLo%z;)c_oyFH+^
zFQ{&H;6VqyG^PVbL}5|e`L>HpJ_W%ydbs3t+ZrOs3$pDAAYAK_I-y6WlLnt1<!ECf
z+n9oK*5-=7_*3HnW{Aw$4m`*SbGjMz6$nt~SJ!g)?%3_yx3BVE7<!S~A3Df2uLvy6
zW&|$!@c3E)A(+rvlDN%hU+(AE9Gi=u-Sei^HvN-WMAt~!vf9aXR005UvI+|UsNm{C
zk!+SRO8T1H+HZFpDPAyq0O9MwSH2^xApOwDfc;_!CJEw87KQ|S4qe#lo(r&Ycvm#X
za>#yC2fPss5hscn)_5RrNrA*zL!v{<3qms-pPLcmENf_)(frHv)0)LrI7YMymm-^g
zxtE$k+!D#^cE-eDV3IC&n3O1MaJ9m{ub&lPJC%4(Q4BbXI9ox-Rujdodo15lR$6+l
zrY34bLbzuqW(BqWIMVd?lC|ef*%Al5PMoisp3FD7+;XXBDU6sV6R`tDj~dR}<(+Xk
z1xaU!0JdJff7poRaUVZFfRMT6b^dz`f9i{;t81)3>SYZb?C(jM!GHjBPPy&QTIKU>
z!(}_jZ8cz1GwV02w;Wq_!uC-hP=JBMyLe&@?sS2KZv76n8Tmx$9XrsYb+W5Y*M2{t
zqLn#d$(&($(24=IO-Jeu=T09t+!-&p+u6HH8c{Cb!aQJ}&UFhi5;h|!CMGBSKzpY&
z0@B@BPZD_9lnX~x(jK0;esAg6eE;0^_*y}=9nL9-dtMv90PWFw*1Z+--cJNUP>tx{
zUdI5Bn$4qGecqgq<oShP80q-ft;OltQCU0_n#+z1(4K{L2e03G@ztwtpX+6d<}@FV
z3eAXvbSIPgjMP<4xFjG-Y3_`79aasj-vhT6joU1!W>p{VKLC$??aj1PCW1Be#v^dV
zvNJ?^_0H#$COnmXS99?*JG_dO3f~=|<qbJRr)Z^3C#u3GhwiOYjc$=b<&Ta5!kId@
zS6q_<0@U#dCJORvq>s~&KL1?%26}DSsFpss|3F^7d+RKpdPf|lx=jr&Vw2IO?*Z-c
zMmt$p2>3uW+EEz@1?XKUDsEctT`~^5z=K*&)L$et%N@KQeq)1cnrQ-3CVJ8^Y%^x8
zK3vkXGxF;W^K0%{+u&P{vgyTj6T$DLvIqilmL<8F7HlE*3%oNM+S^Sn(pZfQ08QFT
z2|PF7PFH!PxS@0a(F#}bR`>?A0ue0uG@TH2k<ur=;Oh`G|A+=RP*cy1#1lPeQ_@gn
z+R)75nr%TM+EWD8nlDcr3>oC<9fmQd(_aj0_kPt;LQI~P$M?HB9_?Cf2Q^P$)pyZ|
zKKjKio#DIr_<)j+>b}hZQ{vUj3i{YG4;&(Wl1)0Bx)~&{wYk2;!BAd0e1tIyevJ`$
zuy(JV*fjrvP&c|)NeumKn8q!-aVFTdI+hYcqI9L^&yaSuXa1ZFer@tqX4R{%?83sG
zvIf$UMcWgMLBM4-52AmL8`b4E0@9_2IUUNzUqc>0#y1Mfmo!Eq_ack=Tb4f}1Qauy
zTaL4)jp2g<c~^@7{t_UA&FW3xScIkxk)Y!5q00dPSz!Cfl<75=0lvLXDB#{So4}Pt
h6K~eH0;1d24M~%iyK4Hy;{W4-LIS4;6i=C(`EM3mI}HE;

literal 0
HcmV?d00001

diff --git a/localizers/easynav_fusion_localizer/maps/square.yaml b/localizers/easynav_fusion_localizer/maps/square.yaml
new file mode 100644
index 00000000..34ce4863
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/maps/square.yaml
@@ -0,0 +1,7 @@
+image: square.png
+mode: trinary
+resolution: 0.2
+origin: [-100.0, -100.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

From 13a55fe73d4c9c9408fe002adf97358c4315258a Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Thu, 5 Mar 2026 12:03:13 +0100
Subject: [PATCH 08/11] params updated

---
 .../config/example_params_summit_dual.yaml    | 42 ++++++++++++++++---
 1 file changed, 36 insertions(+), 6 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
index 482386e8..40b6d005 100644
--- a/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual.yaml
@@ -1,7 +1,7 @@
 controller_node:
   ros__parameters:
     use_sim_time: true
-    controller_types: [mppi]
+    controller_types: [serest]
     dummy:
       rt_freq: 30.0
       plugin: easynav_controller/DummyController
@@ -20,6 +20,30 @@ controller_node:
       max_linear_acceleration: 0.5
       max_angular_acceleration: 0.5
       safety_radius: 0.3
+    serest:
+      rt_freq: 30.0 
+      plugin: easynav_serest_controller/SerestController
+      allow_reverse: true
+      max_linear_speed: 0.8
+      max_angular_speed: 1.2
+      v_progress_min: 0.08        # 8 cm/s de crucero mínimo cuando alineado
+      k_s_share_max: 0.5          # succión lateral no cancela >50% del avance
+      k_theta: 2.5                # sube un poco
+      k_y: 1.5
+      goal_pos_tol: 0.1          # 7 cm
+      goal_yaw_tol_deg: 6.0       # 6 grados
+      slow_radius: 0.60
+      slow_min_speed: 0.03
+      final_align_k: 2.0
+      final_align_wmax: 0.6
+      corner_guard_enable: true
+      corner_gain_ey: 1.8
+      corner_gain_eth: 0.7
+      corner_gain_kappa: 0.4
+      corner_min_alpha: 0.35
+      corner_boost_omega: 1.0
+      a_lat_soft: 0.9
+      apex_ey_des: 0.05
 
 localizer_node:
   ros__parameters:
@@ -48,6 +72,8 @@ localizer_node:
         base_link_frame: base_link
         world_frame: odom
 
+        dynamic_process_noise_covariance: true
+
         #--- INPUT 2: Wheel Odometry ---
         odom0: /robotnik_base_control/odom
         odom0_config:
@@ -213,23 +239,27 @@ maps_manager_node:
 planner_node:
   ros__parameters:
     use_sim_time: true
-    planner_types: [simple]
+    planner_types: [costmap]
     dummy:
       freq: 1.0
       plugin: easynav_planner/DummyPlanner
       cycle_time_nort: 0.2
       cycle_time_rt: 0.0001
-    simple:
+    costmap:
       freq: 10.0
       plugin: easynav_costmap_planner/CostmapPlanner
       cost_factor: 10.0
       continuous_replan: true
+    simple:
+      freq: 0.5
+      plugin: easynav_simple_planner/SimplePlanner
+      robot_radius: 0.3
 
 sensors_node:
   ros__parameters:
     use_sim_time: true
     forget_time: 0.5
-    sensors: [imu, gps, laser1]
+    sensors: [imu, gps]
     perception_default_frame: odom
     imu:
       topic: /imu/data
@@ -246,5 +276,5 @@ sensors_node:
 system_node:
   ros__parameters:
     use_sim_time: true
-    position_tolerance: 0.1
-    angle_tolerance: 0.05
+    position_tolerance: 0.75
+    angle_tolerance: 0.25

From b75fcc47d2da336a92338eeb803c47b4747d6d96 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Fri, 13 Mar 2026 11:35:17 +0100
Subject: [PATCH 09/11] Only starts filter if params are found

---
 .../FusionLocalizer.hpp                       |   4 +-
 .../FusionLocalizer.cpp                       | 196 +++++++++++-------
 2 files changed, 125 insertions(+), 75 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
index 19f829b6..1f49c3f8 100644
--- a/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
+++ b/localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp
@@ -58,6 +58,9 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   std::unique_ptr<robot_localization::UkfWrapper> ukf_local_{nullptr};
   std::unique_ptr<robot_localization::UkfWrapper> ukf_global_{nullptr};
 
+  bool has_global_filter_{false};
+  bool has_local_filter_{false};
+
   int n_gps_sensors_{0};
 
   geometry_msgs::msg::PoseWithCovarianceStamped
@@ -79,7 +82,6 @@ class FusionLocalizer : public easynav::LocalizerMethodBase
   std::vector<rclcpp::Time> last_gps_stamp_;
 
   rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr navsat_pub_{nullptr};
-  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr gps_debug_pub_{nullptr};
   std::string navsatfix_topic_;
 
   bool first_pose_received_{false};
diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index 825c6a09..e9dbee35 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -31,47 +31,89 @@ void FusionLocalizer::on_initialize()
     RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'",
     plugin_name.c_str());
 
-    ukf_global_ = std::make_unique<robot_localization::UkfWrapper>(
-      localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false
-    );
-    ukf_local_ = std::make_unique<robot_localization::UkfWrapper>(
-      localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true
-    );
-    ukf_global_->initialize();
-    ukf_local_->initialize();
-    localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0));
-    localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_);
-
-    localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0));
-    localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_);
-
-    localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0));
-    localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_);
-
-    localizer_node->declare_parameter(
-      plugin_name + ".navsatfix_topic", std::string("global/navsatfix"));
-    localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_);
-    navsat_pub_ = localizer_node->create_publisher<sensor_msgs::msg::NavSatFix>(
-      navsatfix_topic_, rclcpp::QoS(10));
-    gps_debug_pub_ =
-      localizer_node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
-      "gps_pose", rclcpp::QoS(10));
+    // Detect which filters have parameters configured by checking
+    // parameter overrides (loaded from YAML before declaration)
+    const std::string global_prefix = plugin_name + ".global_filter.";
+    const std::string local_prefix = plugin_name + ".local_filter.";
+
+    const auto & overrides =
+      localizer_node->get_node_parameters_interface()->get_parameter_overrides();
+
+    for (const auto & [key, _] : overrides) {
+      if (!has_global_filter_ && key.rfind(global_prefix, 0) == 0) {
+        has_global_filter_ = true;
+      }
+      if (!has_local_filter_ && key.rfind(local_prefix, 0) == 0) {
+        has_local_filter_ = true;
+      }
+      if (has_global_filter_ && has_local_filter_) {
+        break;
+      }
+    }
+
+    if (!has_global_filter_ && !has_local_filter_) {
+      RCLCPP_FATAL(
+        localizer_node->get_logger(),
+        "No parameters found for either '%s' or '%s'. "
+        "At least one filter must be configured.",
+        global_prefix.c_str(), local_prefix.c_str());
+      throw std::runtime_error(
+        "FusionLocalizer: no global_filter or local_filter parameters detected. "
+        "At least one filter must be configured.");
+    }
+
+    if (has_global_filter_) {
+      ukf_global_ = std::make_unique<robot_localization::UkfWrapper>(
+        localizer_node, tf_info.tf_prefix, plugin_name + ".global_filter", false
+      );
+      ukf_global_->initialize();
+    } else {
+      RCLCPP_WARN(localizer_node->get_logger(), "No global_filter parameters found. Global filter will NOT be created.");
+    }
+
+    if (has_local_filter_) {
+      ukf_local_ = std::make_unique<robot_localization::UkfWrapper>(
+        localizer_node, tf_info.tf_prefix, plugin_name + ".local_filter", true
+      );
+      ukf_local_->initialize();
+    } else {
+      RCLCPP_WARN(localizer_node->get_logger(), "No local_filter parameters found. Local filter will NOT be created.");
+    }
+
+    // GPS-related setup only needed when global filter is active
+    if (has_global_filter_) {
+      localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0));
+      localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_);
+
+      localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0));
+      localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_);
+
+      localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0));
+      localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_);
+
+      localizer_node->declare_parameter(
+        plugin_name + ".navsatfix_topic", std::string("gps/filtered"));
+      localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_);
+      navsat_pub_ = localizer_node->create_publisher<sensor_msgs::msg::NavSatFix>(
+        navsatfix_topic_, rclcpp::QoS(10));
+    }
+
   } catch (const std::exception & e) {
     RCLCPP_FATAL(
       get_node()->get_logger(), "Critical failure initializing UkfWrapper: %s",
       e.what());
-    // Raise error
     throw std::runtime_error(std::string("Failed to initialize UkfWrapper: ") + e.what());
   }
 
+  if (has_global_filter_) {
+    GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_,
+        UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_);
+    UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S");
+    UTM_origin_z_ = altitude_origin_;
 
-  GeographicLib::UTMUPS::Forward(latitude_origin_, longitude_origin_, UTM_zone_number_,
-      UTM_zone_northp_, UTM_origin_x_, UTM_origin_y_);
-  UTM_zone_ = std::to_string(UTM_zone_number_) + (UTM_zone_northp_ ? "N" : "S");
-  UTM_origin_z_ = altitude_origin_;
-
-  n_gps_sensors_ = static_cast<int>(ukf_global_->getGpsCallbackDataArr().size());
-  last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME));
+    n_gps_sensors_ = static_cast<int>(ukf_global_->getGpsCallbackDataArr().size());
+    last_gps_stamp_.resize(n_gps_sensors_, rclcpp::Time(0, 0, RCL_ROS_TIME));
+  }
 
   RCLCPP_INFO(get_node()->get_logger(), "FusionLocalizer (UKF) initialized successfully.");
 }
@@ -80,55 +122,61 @@ void FusionLocalizer::update_rt(NavState & nav_state)
 {
   const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();
 
-  if (n_gps_sensors_ && nav_state.has("gnss")) {
-    auto gps_data = nav_state.get<GNSSPerceptions>(std::string("gnss"));
-    const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr();
-    for (int i = 0; i < n_gps_sensors_; ++i) {
-      if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) {
-        continue;
-      }
-      rclcpp::Time gps_time(gps_data[i]->data.header.stamp);
-      if (gps_time > last_gps_stamp_[i]) {
-        EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps");
-        last_gps_stamp_[i] = gps_time;
-        auto pose =
-          std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(navsatfix_to_pose(
-            gps_data[i]->data));
-        if (!first_pose_received_) {
-          RCLCPP_INFO(get_node()->get_logger(),
-              "First valid GPS fix received. Initializing filter state.");
-          if(nav_state.has("imu")) {
-            auto imu_data = nav_state.get<IMUPerceptions>(std::string("imu"));
-            if (!imu_data.empty()) {
-              pose->pose.pose.orientation = imu_data[0]->data.orientation;
+  if (has_global_filter_) {
+    if (n_gps_sensors_ && nav_state.has("gnss")) {
+      auto gps_data = nav_state.get<GNSSPerceptions>(std::string("gnss"));
+      const auto & gps_cb_arr = ukf_global_->getGpsCallbackDataArr();
+      for (int i = 0; i < n_gps_sensors_; ++i) {
+        if (gps_data[i]->data.status.status < sensor_msgs::msg::NavSatStatus::STATUS_FIX) {
+          continue;
+        }
+        rclcpp::Time gps_time(gps_data[i]->data.header.stamp);
+        if (gps_time > last_gps_stamp_[i]) {
+          EASYNAV_TRACE_NAMED_EVENT("fusion_localizer_process_gps");
+          last_gps_stamp_[i] = gps_time;
+          auto pose =
+            std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(navsatfix_to_pose(
+              gps_data[i]->data));
+          if (!first_pose_received_) {
+            RCLCPP_INFO(get_node()->get_logger(),
+                "First valid GPS fix received. Initializing filter state.");
+            if(nav_state.has("imu")) {
+              auto imu_data = nav_state.get<IMUPerceptions>(std::string("imu"));
+              if (!imu_data.empty()) {
+                pose->pose.pose.orientation = imu_data[0]->data.orientation;
+              }
             }
+            ukf_global_->setPoseCallback(pose);
+            first_pose_received_ = true;
+            continue;
           }
-          ukf_global_->setPoseCallback(pose);
-          first_pose_received_ = true;
-          continue;
+          ukf_global_->poseCallback(
+            pose,
+            gps_cb_arr[i],
+            tf_info.map_frame,
+            gps_data[i]->data.header.frame_id,
+            false
+          );
         }
-        gps_debug_pub_->publish(*pose);
-        ukf_global_->poseCallback(
-          pose,
-          gps_cb_arr[i],
-          tf_info.map_frame,
-          gps_data[i]->data.header.frame_id,
-          false
-        );
       }
     }
-  }
 
-  ukf_global_->periodicUpdate();
-  ukf_local_->periodicUpdate();
+    ukf_global_->periodicUpdate();
 
-  nav_msgs::msg::Odometry global_odom, local_odom;
-  if (ukf_global_->getFilteredOdometryMessage(&global_odom)) {
-    nav_state.set("robot_pose", global_odom);
-    navsat_pub_->publish(odom_to_navsatfix(global_odom));
+    nav_msgs::msg::Odometry global_odom;
+    if (ukf_global_->getFilteredOdometryMessage(&global_odom)) {
+      nav_state.set("robot_pose", global_odom);
+      navsat_pub_->publish(odom_to_navsatfix(global_odom));
+    }
   }
-  if (ukf_local_->getFilteredOdometryMessage(&local_odom)) {
-    nav_state.set("robot_pose_local", local_odom);
+
+  if (has_local_filter_) {
+    ukf_local_->periodicUpdate();
+
+    nav_msgs::msg::Odometry local_odom;
+    if (ukf_local_->getFilteredOdometryMessage(&local_odom)) {
+      nav_state.set("robot_pose_local", local_odom);
+    }
   }
 }
 

From f2bfb9bb02d60506018b452dec848a9736c93d5b Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Fri, 13 Mar 2026 11:36:57 +0100
Subject: [PATCH 10/11] Uncrustify changes

---
 .../src/easynav_fusion_localizer/FusionLocalizer.cpp        | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
index e9dbee35..7982a9a7 100644
--- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
+++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp
@@ -68,7 +68,8 @@ void FusionLocalizer::on_initialize()
       );
       ukf_global_->initialize();
     } else {
-      RCLCPP_WARN(localizer_node->get_logger(), "No global_filter parameters found. Global filter will NOT be created.");
+      RCLCPP_WARN(localizer_node->get_logger(),
+          "No global_filter parameters found. Global filter will NOT be created.");
     }
 
     if (has_local_filter_) {
@@ -77,7 +78,8 @@ void FusionLocalizer::on_initialize()
       );
       ukf_local_->initialize();
     } else {
-      RCLCPP_WARN(localizer_node->get_logger(), "No local_filter parameters found. Local filter will NOT be created.");
+      RCLCPP_WARN(localizer_node->get_logger(),
+          "No local_filter parameters found. Local filter will NOT be created.");
     }
 
     // GPS-related setup only needed when global filter is active

From 1a70c12eb57485de120e267b4e1b9a90e77f7ad4 Mon Sep 17 00:00:00 2001
From: migueldm <midemig@gmail.com>
Date: Fri, 13 Mar 2026 11:37:51 +0100
Subject: [PATCH 11/11] Added config file for gazebo smulation demo

---
 .../example_params_summit_dual_gazebo.yaml    | 280 ++++++++++++++++++
 1 file changed, 280 insertions(+)
 create mode 100644 localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml

diff --git a/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml
new file mode 100644
index 00000000..b7afad57
--- /dev/null
+++ b/localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml
@@ -0,0 +1,280 @@
+controller_node:
+  ros__parameters:
+    use_sim_time: true
+    controller_types: [serest]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_controller/DummyController
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.0001
+    mppi:
+      rt_freq: 30.0 
+      plugin: easynav_mppi_controller/MPPIController
+      num_samples: 200
+      horizon_steps: 5
+      dt: 0.15
+      lambda: 1.0      
+      fov: 1.57
+      max_linear_velocity: 1.5
+      max_angular_velocity: 1.0
+      max_linear_acceleration: 0.5
+      max_angular_acceleration: 0.5
+      safety_radius: 0.3
+    serest:
+      rt_freq: 30.0 
+      plugin: easynav_serest_controller/SerestController
+      allow_reverse: true
+      max_linear_speed: 0.8
+      max_angular_speed: 1.2
+      v_progress_min: 0.08        # 8 cm/s de crucero mínimo cuando alineado
+      k_s_share_max: 0.5          # succión lateral no cancela >50% del avance
+      k_theta: 2.5                # sube un poco
+      k_y: 1.5
+      goal_pos_tol: 0.1          # 7 cm
+      goal_yaw_tol_deg: 6.0       # 6 grados
+      slow_radius: 0.60
+      slow_min_speed: 0.03
+      final_align_k: 2.0
+      final_align_wmax: 0.6
+      corner_guard_enable: true
+      corner_gain_ey: 1.8
+      corner_gain_eth: 0.7
+      corner_gain_kappa: 0.4
+      corner_min_alpha: 0.35
+      corner_boost_omega: 1.0
+      a_lat_soft: 0.9
+      apex_ey_des: 0.05
+
+localizer_node:
+  ros__parameters:
+    use_sim_time: true
+    localizer_types: [Ukf]
+    dummy:
+      rt_freq: 30.0
+      plugin: easynav_localizer/DummyLocalizer
+      cycle_time_nort: 0.01
+      cycle_time_rt: 0.001
+    Ukf:
+      rt_freq: 50.0
+      freq: 5.0
+      reseed_freq: 0.1
+      plugin: easynav_fusion_localizer/FusionLocalizer
+      latitude_origin: 40.284796759605115
+      longitude_origin: -3.8214099132625017
+      altitude_origin: 669.9730248888955
+      local_filter:
+        two_d_mode: true
+        publish_tf: false
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: odom
+
+        dynamic_process_noise_covariance: true
+
+        #--- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [true,  true, false,
+          false, false, true,
+          true,  true, false,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        # imu0: /imu/data
+        # imu0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   false, false, false,
+        #   false, false, true,
+        #   true, true, true]
+        # imu0_differential: false
+        # imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  false,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        process_noise_covariance: [0.05, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.05, 0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.06, 0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.06, 0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.025, 0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.025, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.04, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.01, 0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.01, 0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.02, 0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.01, 0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.01, 0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.015]
+      global_filter:
+        two_d_mode: true
+        publish_tf: true
+        debug: true
+        debug_out_file: /tmp/ukf_global_debug.txt
+        transform_timeout: 0.1
+        smooth_lagged_data: true
+        # Defines the time window in seconds to keep past states in memory.
+        # history_length: 5.0
+        dynamic_process_noise_covariance: true
+
+        # --- Reference Frames ---
+        map_frame: map
+        odom_frame: odom
+        base_link_frame: base_link
+        world_frame: map
+
+        # --- INPUT 1: GPS Odometry ---
+        gps0: /gps/fix
+        gps0_config:
+          [true,  true,  false,
+          false, false, false,
+          false, false, false,
+          false, false, false,
+          false, false, false]
+        gps0_differential: false
+
+        # --- INPUT 2: Wheel Odometry ---
+        odom0: /robotnik_base_control/odom
+        odom0_config:
+          [false, false, false,
+          false, false, false,
+          true,  true,  false,
+          false, false, true,
+          false, false, false]
+        odom0_differential: false
+
+        # --- INPUT 3: IMU ---
+        imu0: /imu/data
+        imu0_config:
+          [false, false, false,
+          false, false, false,
+          false, false, false,
+          false, false, true,
+          false, false, false]
+        imu0_differential: false
+        imu0_remove_gravitational_acceleration: true
+
+        # twist0: /gps/fix_velocity
+        # twist0_config:
+        #   [false, false, false,
+        #   false, false, false,
+        #   true,  true,  false,
+        #   false, false, false,
+        #   false, false, false]
+        # twist0_differential: false
+
+        # --- Process Variances ---
+        process_noise_covariance: [0.05, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.05, 0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.06, 0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.03, 0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.06, 0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.025, 0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.025, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.04, 0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.01, 0.0,  0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.01, 0.0,  0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.02, 0.0,  0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.01, 0.0,  0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.01, 0.0,
+                                  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,   0.0,   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.015]
+
+        initial_estimate_covariance: [
+          5.0, 5.0, 1e-3,   1e-3, 1e-3, 1.0,
+          0.5,  0.5,  1e-3,   1e-3, 1e-3, 0.1,
+          0.1,  0.1,  0.1
+        ]
+
+
+maps_manager_node:
+  ros__parameters:
+    use_sim_time: true
+    map_types: [costmap, routes]
+    routes:
+      freq: 10.0
+      plugin: easynav_routes_maps_manager/RoutesMapsManager
+      filters: [routes_costmap]
+      package: easynav_fusion_localizer
+      map_path_file: routes/simple_square.yaml
+      routes_costmap:
+        plugin: easynav_routes_maps_manager/RoutesCostmapFilter
+        min_cost: 50
+        route_width: 0.5
+      cycle_time_nort: 0.001
+      cycle_time_rt: 0.001
+    costmap:
+      freq: 10.0
+      plugin: easynav_costmap_maps_manager/CostmapMapsManager
+      package: easynav_fusion_localizer
+      map_path_file: maps/empty.yaml
+      filters: [obstacles, inflation]
+      obstacles:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/ObstaclesFilter
+      inflation:
+        plugin: easynav_costmap_maps_manager/CostmapMapsManager/InflationFilter
+        inflation_radius: 1.3
+        inscribed_radius: 0.25
+        cost_scaling_factor: 3.0
+    dummy:
+      freq: 10.0
+      plugin: easynav_maps_manager/DummyMapsManager
+      cycle_time_nort: 0.1
+      cycle_time_rt: 0.0001
+
+planner_node:
+  ros__parameters:
+    use_sim_time: true
+    planner_types: [costmap]
+    dummy:
+      freq: 1.0
+      plugin: easynav_planner/DummyPlanner
+      cycle_time_nort: 0.2
+      cycle_time_rt: 0.0001
+    costmap:
+      freq: 10.0
+      plugin: easynav_costmap_planner/CostmapPlanner
+      cost_factor: 10.0
+      continuous_replan: true
+    simple:
+      freq: 0.5
+      plugin: easynav_simple_planner/SimplePlanner
+      robot_radius: 0.3
+
+sensors_node:
+  ros__parameters:
+    use_sim_time: true
+    forget_time: 0.5
+    sensors: [imu, gps]
+    perception_default_frame: odom
+    imu:
+      topic: /imu/data
+      type: sensor_msgs/msg/Imu
+    gps:
+      topic: /gps/fix
+      type: sensor_msgs/msg/NavSatFix
+    laser1:
+      topic: /front_laser/points
+      type: sensor_msgs/msg/PointCloud2
+      group: points
+    
+
+system_node:
+  ros__parameters:
+    use_sim_time: true
+    position_tolerance: 0.75
+    angle_tolerance: 0.25