[Autoware 1.12.0] astar_avoid can't correct closest waypoint in some cases

edit

Required information:

• Operating system and version: Ubuntu 16.04
• Autoware installation type: From source
• Autoware version or commit hash: tag v.1.12
• ROS distribution and version: Kinetic
• ROS installation type: from binaries
• Package or library, if applicable: N/A

Description of the bug

We plan path using way_palnner, waypoint_planner and waypoint_follower. After vehicle reached goal, astar will calculate incorrect closest waypoint if we want to plan new path

The error messages is:

no candidate. search closest waypoint from all waypoints...

no candidate. search closest waypoint from all waypoints...

no candidate. search closest waypoint from all waypoints...

[checkInitialized] initialized: 1

Please refer to below video, rosbag and ros log for how to reproduce this issue:

video: https://drive.google.com/open?id=1nCE...

rosbag: https://drive.google.com/open?id=1mal...

ros log: https://drive.google.com/open?id=1X6O...

Any response will be greatly appreciated! Many thanks for your help.