Deactivate localization during pose initialization

[kminoda]

Background

Current localization architecture has an issue when re-initializing the pose. If we want to re-initialize from point A to B, we cannot guarantee that both NDT and EKF states are updated to point B synchronously. This may end up in the localization estimation being pulled back to point A.

For example, here's what may happen:

1. pose_initializer sends a pose initialization request to NDT (EKF: A)
2. pose_initializer receives an accurate initial pose from NDT (EKF: A)
3. NDT receives /loc.../pose_twist_.../biased_pose_with_covariance = point A initial guess from EKF (EKF: A) (which should not happen)
4. NDT keeps publishing point A (EKF: A)
5. /initialpose3d is sent from pose_initializer to EKF (EKF: B)
6. EKF receives /loc.../pose_estimator/pose_with_covariance = point A from NDT (EKF: A)

Proposal

Here I make an assumption that the pose initialization is only performed when either of the following is met:

• the vehicle is stopped
• the vehicle is disengaged (or driven manually)

In other words, I do not consider the pose re-initialization when the vehicle is driving autonomously. (I believe that re-initialization during autonomous driving is rather a long-term challenge, so I leave it out of the scope for now)

To overcome this issue, I propose to deactivate the localization components (in current Universe, ndt_scan_matcher and ekf_localizer) during the pose initialization. When they are deactivated, the localization is expected to stop publishing the estimation. After the calculation finishes, the components throw away their pose&twist buffers they have and are activated.

PR

autowarefoundation/autoware.universe#1873