AR_WPNav: Rover 4.3+ does not honor WP_RADIUS

[yuri-rage]

ardupilot/libraries/AR_WPNav/AR_WPNav.cpp

Line 459 in db7fad0

_reached_destination = near_wp || past_wp;

The above referenced line is only ever reached when the vehicle has passed the destination waypoint (a result of s_finished remaining false until enough time has elapsed to have arrived at the destination). Thus, there is always an overshoot of the desired waypoint, which necessitates a course correction upon pivot completion.

If I change the logic to ignore s_finished except during fast waypoints, then _reached_destination becomes true in a timely manner, but it seems the controller is still awaiting time elapsed on that leg before initiating a pivot, and the resulting behavior is actually worse:

    // check if we've reached the waypoint
    if (!_reached_destination) {
        // "fast" waypoints are complete once the intermediate point reaches the destination
        if (_fast_waypoint && s_finished) {
            _reached_destination = true;
        } else {
            // regular waypoints also require the vehicle to be within the waypoint radius or past the "finish line"
            const bool near_wp = current_loc.get_distance(_destination) <= _radius;
            const bool past_wp = current_loc.past_interval_finish_line(_origin, _destination);
            _reached_destination = near_wp || past_wp;
        }
    }

On the other hand, if I use a Lua script (workaround) to force the WP ahead when WP_RADIUS is satisfied, the vehicle behaves extremely well, so I'm certain that I'm on the right track.

I suspect this is among the chief reasons that the community has had a hard time tuning the new nav controller. Any vehicle that is slightly "loose" in deceleration will overshoot nearly every waypoint, sometimes by a significant margin, causing path control issues thereafter.

[xianglunkai]

I think you're right. Last year, I conducted a investigation into Scuve NAV for a long time.
In addition, the new navigation is difficult to solve the obstacle avoidance problem with relatively low acceleration（<=0.1m/s2）; And the issue of returning to the route after avoiding obstacles, although I have done some work, I believe that it is not coordinated and graceful enough within the current framework

[xianglunkai]

#22957

[rmackay9]

Thanks for the investigation. The SCurves will slow their "time" state to handle slow moving vehicles but it doesn't support speeding up time to handle overspeeding vehicles.. the idea was that a vehicle can always slow down but it may not be able to speed up... maybe that assumption is incorrect and we should allow time to being sped up slowed down... within limits of course.

[yuri-rage]

Rather than a speed inaccuracy, I think the problem may be that segment time is calculated to the waypoint itself rather than waypoint distance minus radius. I couldn't quite find where the end_time value is assigned, so that is only a guess.

[rmackay9]

We've merged PR #25623 which I think resolves the overshoot issue so I will go ahead and close this as is normal when we merge a fix to master.

As a follow-up I will also backport the above fix and release as a Rover beta release in the coming week