Accept Initial Pose Without Performing NDT Alignment

[angry-crab]

This is further discussion of Pose Initialization for Bag Play.
As demonstrated by Yamato-san, to set an initial pose during random bag replay, the procedure is as follows:

1. Play a bag
2. Pause rosbag and wait for NDT to finish
3. restart playing

The reason we have to start playing is because we need a point cloud frame to be published so that we can use it for NDT. I think we might want to accept an initial pose sent from Rviz without performing NDT, since it is more user-friendly. Further, as long as the initial pose is reasonable, NDT will be able to correct the pose afterwards. I guess the major concern of accepting an unaligned initial pose would be that once we have false-positive localization result, this could lead to unpredictable behavior. Please let me know your thoughts.

[YamatoAndo]

@angry-crab
If the topic name of the 2d pose estimate is /initialpose3d, the initial position can be given directly without running NDT, by the way.