xy_goal_toleranceとyaw_goal_toleranceを動的に変更できるようにしたい

[Alpaca-zip]

概要
waypoint_navigationとnavigation2でゴール判定が違うためコードの保守性が悪く、バグ*1を踏んでいそう。

改善案
navigation2ではゴール判定を動的に変えられるようになっているので(以下参照)、waypoint_navigation側でxy_goal_toleranceおよびyaw_goal_toleranceをsetしてnav_status_ == rclcpp_action::ResultCode::SUCCEEDEDのみで判定するようにできそう。

https://github.com/ros-planning/navigation2/blob/4bc3e8a534005e983ee7a49a09ec00aeb9820661/nav2_controller/plugins/simple_goal_checker.cpp#L144-L166

具体的には、xy_goal_toleranceにradを、yaw_goal_toleranceには $\pi$ とか $2\pi$ を設定して、stop が true のときはmin_yaw_errの値を設定できるようにしたい。