Reset pose from RViz2 for every localizer

[fmrico]

PR: Normalize /initialpose across localizers + functional tests (AMCL/GPS/Fusion)

Context / motivation

Within easynav_plugins/localizers, plugins differed in how they consumed the standard /initialpose topic (RViz-style initial pose). This made integration harder and prevented consistent validation that localizers react correctly to:

• Parameter-based initialization (seed pose different from 0).
• Runtime updates via /initialpose.

What changed

1) Consistent /initialpose subscription in all localizers

All plugins under src/easynav_plugins/localizers were reviewed and normalized so that:

• All of them create a subscription to /initialpose (geometry_msgs/msg/PoseWithCovarianceStamped).

Localizers covered:

• AMCL (costmap): easynav_costmap_localizer
• AMCL (simple): easynav_simple_localizer
• AMCL (navmap): easynav_navmap_localizer
• GPS: easynav_gps_localizer
• Fusion: easynav_fusion_localizer

2) Functional tests to validate end-to-end behavior

Tests were migrated/re-structured into a consistent functional pattern (GTest) to cover:

1. Localizer initialization with a parameter-based seed pose different from zero.
2. Reading the state/estimated pose (getter or NavState).
3. Publishing /initialpose.
4. Verifying that the state changes after processing the message.

Updated/added tests:

• AMCL:
  • easynav_costmap_localizer/tests/*
  • easynav_simple_localizer/tests/*
  • easynav_navmap_localizer/tests/*
• GPS:
  • easynav_gps_localizer/tests/gps_localizer_tests.cpp
• Fusion:
  • easynav_fusion_localizer/tests/fusion_localizer_tests.cpp

3) Parameter-based initial pose seed in GPS and Fusion

To enable deterministic initialization (and make it testable), the following parameters are now read:

• <plugin>.initial_pose.x
• <plugin>.initial_pose.y
• <plugin>.initial_pose.yaw

Implemented in:

• easynav_gps_localizer (preloads the initial pose)
• easynav_fusion_localizer (preloads / applies the initial pose to the filter(s))

4) Fix: /initialpose must be reflected deterministically in Fusion (UKF)

An issue was found where Fusion would receive /initialpose, but the output (NavState[robot_pose]) would not reliably update due to how the UKF set-pose path was handled.

robot_localization::UkfWrapper::setPoseCallback() was updated so that “set pose”:

• Forces the filter internal state deterministically.
• Only calls processMeasurement() if the filter was not initialized yet.

This makes /initialpose impact Fusion’s published pose immediately and enables robust functional tests.

Please, check it @midemig

Compatibility notes

• No user-facing public API changes (standard topics/messages), but behavior is now more consistent.
• GPS/Fusion can now optionally seed an initial pose via parameters; if unset, the default behavior remains.

Checklist

• All localizers subscribe to /initialpose.
• Subscription is created without SubscriptionOptions (no explicit callback group).
• Functional tests cover parameter-seeded init + /initialpose updates.
• Fusion reflects /initialpose deterministically in robot_pose.

[Copilot]

Pull request overview

Normalizes RViz2 /initialpose handling across multiple EasyNav localizers and adds functional tests to validate parameter-seeded initialization and runtime pose resets.

Changes:

• Added /initialpose subscriptions and callbacks to AMCL-based localizers and to GPS/Fusion localizers.
• Added/updated GTest-based functional tests for AMCL (simple/navmap/costmap), GPS, and Fusion localizers.
• Updated Fusion UKF set-pose behavior to make /initialpose updates deterministic.

Reviewed changes

Copilot reviewed 28 out of 28 changed files in this pull request and generated 4 comments.

Show a summary per file

File	Description
maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp	Removes debug std::cout noise during updates.
localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp	Replaces previous tests with functional /initialpose behavior test.
localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp	Adds /initialpose subscription + particle reinitialization callback; safer callback group fallback.
localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp	Declares /initialpose subscriber and callback.
localizers/easynav_simple_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp	New functional test validating seed pose + /initialpose update.
localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp	Removes older tests (dynamic update/map/service) in favor of functional pattern.
localizers/easynav_navmap_localizer/tests/CMakeLists.txt	Renames/rewires gtest target to new navmap test file.
localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp	Adds /initialpose subscription + callback; safer callback group fallback.
localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp	Declares /initialpose subscriber and callback.
localizers/easynav_navmap_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp	New functional test validating parameter seed + /initialpose alignment behavior.
localizers/easynav_gps_localizer/tests/CMakeLists.txt	Adds gtest target for GPS localizer functional test.
localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp	Adds /initialpose subscription, optional parameter seed, and origin alignment on first valid fix.
localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp	Adds /initialpose subscriber + pending init pose storage.
localizers/easynav_gps_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp	New functional test validating parameter seed + /initialpose update reflected in output odometry.
localizers/easynav_fusion_localizer/tests/CMakeLists.txt	Adds gtest target for Fusion localizer functional test.
localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp	Makes set-pose deterministic and initializes filter only when needed.
localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp	Adds /initialpose subscription + callback and optional parameter-based seed pose application.
localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp	Declares /initialpose subscriber and callback.
localizers/easynav_fusion_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp	Updates test suite to functional seed + /initialpose reset pattern.
localizers/easynav_costmap_localizer/CMakeLists.txt	Enables building tests subdirectory.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

FusionLocalizer.cpp now uses std::abs(...) for the initial pose parameter check, but the file doesn’t include <cmath>. Please include <cmath> explicitly to avoid relying on transitive includes / accidental overload resolution.

[Copilot]

FusionLocalizer.cpp now directly includes/uses tf2_geometry_msgs (e.g., tf2::toMsg for seeding and /initialpose forwarding). Please ensure localizers/easynav_fusion_localizer/CMakeLists.txt declares tf2/tf2_geometry_msgs as direct dependencies (via find_package(...) and linking/exporting the targets), rather than relying on transitive dependencies from robot_localization.

[Copilot]

GpsLocalizer.cpp now uses std::abs(...) (e.g., for the non-zero initial pose check) but this translation unit doesn’t include <cmath>. Please include <cmath> explicitly (or otherwise ensure the correct overloads are declared) to avoid relying on transitive includes / potential build failures on different toolchains.

[Copilot]

GpsLocalizer now directly depends on geometry_msgs (/initialpose message type) and tf2_geometry_msgs (tf2::toMsg), but localizers/easynav_gps_localizer/CMakeLists.txt does not declare those dependencies via find_package(...) / exported targets. Please add the missing dependencies and link/export them so this header remains consumable without relying on transitive includes.

Suggested change

	#include "tf2_ros/static_transform_broadcaster.hpp"
	#include "tf2_ros/static_transform_broadcaster.hpp"
	#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"