Initpose from rviz2

[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

This PR aims to standardize how EasyNav localizer plugins consume the RViz-style initial pose topic and adds/updates functional GTests to validate parameter-seeded initialization plus runtime updates via /initialpose.

Changes:

• Added /initialpose subscriptions and callbacks to multiple localizers (AMCL variants, GPS, Fusion) and documented the interface.
• Reworked/added functional tests to validate deterministic seeding and runtime pose resets.
• Updated Fusion’s UKF set-pose path to deterministically update filter state.

Reviewed changes

Copilot reviewed 29 out of 29 changed files in this pull request and generated 8 comments.

Show a summary per file

File	Description
maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp	Removes leftover debug prints.
localizers/easynav_simple_localizer/tests/simple_localizer_tests.cpp	Replaces prior tests with functional /initialpose-driven initialization/reset test.
localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp	Adds /initialpose subscription + callback that reinitializes particles from pose/covariance.
localizers/easynav_simple_localizer/include/easynav_simple_localizer/AMCLLocalizer.hpp	Declares initial pose subscription + callback.
localizers/easynav_simple_localizer/README.md	Documents initial pose subscription interface.
localizers/easynav_simple_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_navmap_localizer/tests/navmap_localizer_tests.cpp	Adds functional /initialpose initialization/reset test.
localizers/easynav_navmap_localizer/tests/costmap_localizer_tests.cpp	Removes older navmap tests (dynamic update/map/service).
localizers/easynav_navmap_localizer/tests/CMakeLists.txt	Switches test target to new navmap localizer test.
localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp	Adds /initialpose subscription + callback mirroring simple AMCL behavior.
localizers/easynav_navmap_localizer/include/easynav_navmap_localizer/AMCLLocalizer.hpp	Declares initial pose subscription + callback.
localizers/easynav_navmap_localizer/README.md	Documents initial pose subscription interface.
localizers/easynav_navmap_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_gps_localizer/tests/gps_localizer_tests.cpp	Adds functional test for param-seeded init + /initialpose reset.
localizers/easynav_gps_localizer/tests/CMakeLists.txt	Adds gtest target for GPS localizer tests.
localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp	Adds /initialpose subscription and parameter-based initial pose staging to align UTM origin.
localizers/easynav_gps_localizer/include/easynav_gps_localizer/GpsLocalizer.hpp	Adds initial pose subscription + pending init pose storage.
localizers/easynav_gps_localizer/README.md	Documents initial pose params and subscription interface.
localizers/easynav_gps_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_fusion_localizer/tests/fusion_localizer_tests.cpp	Adds functional test expecting param-seeded init + /initialpose reset.
localizers/easynav_fusion_localizer/tests/CMakeLists.txt	Adds gtest target for Fusion localizer tests.
localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp	Makes set-pose deterministically set filter state; only processes measurement if uninitialized.
localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp	Adds /initialpose subscription and forwards pose to filters; adjusts GNSS NavState access pattern.
localizers/easynav_fusion_localizer/include/easynav_fusion_localizer/FusionLocalizer.hpp	Declares initial pose subscription + callback.
localizers/easynav_fusion_localizer/config/example_params_summit_dual_gazebo.yaml	Assigns GNSS sensor to gnss group.
localizers/easynav_fusion_localizer/README.md	Updates package name/docs; documents initial pose parameters and subscription.
localizers/easynav_fusion_localizer/CMakeLists.txt	Enables building tests subdirectory.
localizers/easynav_costmap_localizer/tests/costmap_localizer_tests.cpp	Replaces prior tests with functional /initialpose initialization/reset test.
localizers/easynav_costmap_localizer/CMakeLists.txt	Enables building tests subdirectory.

Comments suppressed due to low confidence (2)

localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp:39

• FusionLocalizer doesn’t currently declare/read the documented <plugin>.initial_pose.{x,y,yaw} parameters or apply a seed pose during startup. This makes parameter-based deterministic initialization (and the new functional test expectation) ineffective until an /initialpose message arrives. Consider reading these parameters in on_initialize() and calling ukf_global_/ukf_local_->setPoseCallback() (or equivalent) to seed the filters.

    const std::string & plugin_name = this->get_plugin_name();
    const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info();

    RCLCPP_INFO(localizer_node->get_logger(), "Using tf_prefix: '%s'", tf_info.tf_prefix.c_str());
    RCLCPP_INFO(localizer_node->get_logger(), "Using parameter namespace: '%s'",

localizers/easynav_navmap_localizer/tests/CMakeLists.txt:10

• ${std_srvs_TARGETS} is linked into this test target but nothing in navmap_localizer_tests.cpp appears to require it anymore. Remove it if unused to keep the link interface minimal.

target_link_libraries(navmap_localizer_tests
  ${PROJECT_NAME}
  rclcpp::rclcpp
  rclcpp_lifecycle::rclcpp_lifecycle
  ${std_srvs_TARGETS}

---

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