Add Ability to Pass Initial Guess for Localization via RViz2
Description
NDT localization needs an "initial guess" to narrow the search domain for generating a pose estimate. Currently this is done through a "hack" where we pass the initial guess through parameters. Long-term, this should be provided either by the user with Rviz2 or from GPS. This issue is for implementing the first method.
Purpose
Without an initial guess, NDT can not localize. With the existing hack, we cannot localize outside of a specific map.
Desired Behavior
The NDT localizer should be able to accept coordinates in the map
frame generated from the "2D Pose Estimate" tool in Rviz2 to be used as the initial guess.
Definition of Done
-
Code added to accept coordinates from Rviz2 tool -
Tested thoroughly in simulation -
MS3 Rviz2 config file updated to use map
frame as fixed frame