[ROS2][Eloquent] The Robot gets stuck in Waypoint mode

[seanyen]

Describe the bug If you plan multiple waypoints and execute on it in RViz2, the robot may navigate to the first point, then gets stuck, or the BT cannot tell the robot already achieves the next goal.

And if you reset and restart the waypoint mode, you won't be able to plan multiple waypoints and instead you will see many BT error messages put in the console.