Pose Initialization for Bag Play

[angry-crab]

What is the correct way to initialize a pose for bag replay? The following messages were displayed when I tried to set an initial pose using Rviz.

[pose_initializer-6] [INFO] [1653292237.940868029] [localization.util.pose_initializer]: call NDT Align Server
[ndt_scan_matcher-7] [WARN] [1653292237.941212200] [localization.pose_estimator.ndt_scan_matcher]: No InputSource
[pose_initializer-6] [INFO] [1653292237.941403890] [localization.util.pose_initializer]: failed NDT Align Server

I guess this is because it is assuming that messages from lidar are constantly available, but this is not true for bag replay. Is such behavior intended? Or do we have another way to set the initial pose?

[YamatoAndo]

This error indicates that the messages from lidar have not been received.
If the messages are received properly, it will work with rosbag replay.
Would you please check if the messages are published? (default topic name is /localization/util/downsample/pointcloud)

[angry-crab]

Yes, but it is because I was not playing the bag at that time. The initial pose(maybe just a guess) should be set before we have sensor messages. For the rosbag I used in this case, the vehicle starts moving right after playing the bag. And it does not make any sense to set an initial pose for a moving vehicle.

[angry-crab]

kitti_bag
I have shared my testing data if there is anyone would like to try out.

[YamatoAndo]

There are many things that need to be fixed, but I will just point out the bare minimum for initial position estimation.

1. Set QoS of /localization/util/downsample/pointcloud to best_effort.
   https://docs.ros.org/en/foxy/How-To-Guides/Overriding-QoS-Policies-For-Recording-And-Playback.html

2. Downsample the sensor point cloud because there are too many sensor point clouds and the process is too slow.

3. Use the following command to pause rosbag and wait for the initial position estimation to finish.
   ros2 service call /rosbag2_player/toggle_paused rosbag2_interfaces/srv/TogglePaused

[angry-crab]

Thank you for the information. Were you able to localize the vehicle using this dataset?

[YamatoAndo]

Yes.
If it is not working well, I can help you.

[YamatoAndo]

demo video using kitti_bag

kitti_for_initial_pose_estimation-2022-05-24_04.07.02.mp4

[gayar-helm]

Hello,
@angry-crab have you been able to reproduce @YamatoAndo 's demo?
I think I am experiencing similar issue here #2749