Pure Pursuit: Handle edge case and add logging message #24084
Conversation
chfriedrich98
force-pushed
the
purepursuit_message
branch
3 times, most recently
from
f0b1493 to
c90e831
Compare
chfriedrich98
force-pushed
the
purepursuit_message
branch
from
c90e831 to
c8019bc
Compare
There was a problem hiding this comment.
Maybe since you have distance_to_waypoint, also add bearing_to_waypoint. Both are part of position_controller_status uORB and NAV_CONTROLLER_OUTPUT MAVLink.
|
|
||
| float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller | ||
| float32 target_bearing # [rad] Target bearing calculated by the pure pursuit controller | ||
| float32 crosstrack_error # [m] Shortest distance from the vehicle to the path (Positiv: Right of the path, Negativ: Left of the path) |
There was a problem hiding this comment.
Positive and Negative. Maybe add a bit more exact description what left means, signum of the crosstrack_error matters on the direction if the path. I would consider something like: Value is negative, if vehicle is on a left hand side with respect to the oriented path vector.
| prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp | ||
| const Vector2f curr_pos_to_path = _distance_along_path - | ||
| prev_wp_to_curr_pos; // Shortest vector from the current position to the path | ||
| _crosstrack_error = sign(prev_wp_to_curr_wp(1) * curr_pos_to_path( |
There was a problem hiding this comment.
I think this can use just sign(curr_pos_to_path.dot(curr_pos_to_curr_wp))*curr_pos_to_path.norm()
Dot product can be used to measure an angle, between two vectors. But I reckon yours should also work.
| const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero(); | ||
| _distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u; | ||
| _curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos; | ||
| } else { |
There was a problem hiding this comment.
Does not need else statement block, as the if is short-circuiting to return statement
| } | ||
|
|
||
|
|
||
| } else { |
There was a problem hiding this comment.
Same, does not need else as the previous if-elseif-else is catch all with returns
| float getCrosstrackError() {return _curr_pos_to_path.norm();}; | ||
| float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();}; | ||
| float getDistanceAlongPath() {return _distance_along_path.norm();}; | ||
| float getCrosstrackError() {return _crosstrack_error;}; | ||
|
|
There was a problem hiding this comment.
Add getter method for targetBearing and current WP bearing?
| float _lookahead_distance{NAN}; // Radius of the circle around the vehicle | ||
| float _crosstrack_error{NAN}; // Shortest distance from the current position to the path (Positiv: right of path, Negativ: left of path) | ||
| Vector2f _curr_pos_to_curr_wp{}; | ||
| Vector2f _distance_along_path{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp |
There was a problem hiding this comment.
_distance_along_path I would assume is a single float, denoting a "distance". Maybe reconsider _position_along_path?
There was a problem hiding this comment.
Maybe consider adding tests testing also _crosstrack_error tests.
Solved Problem
Pure pursuit edge case
The pure pursuit library can now handle the following edge case:
If the acceptance radius of a waypoint is bigger than the lookahead radius of the pure pursuit controller the following edge case can happen:
The linesegment is outside of the lookahead which makes the rover default to calculating the bearing towards the closest point on the extended line segment (red dot). However, in this edge case this leads the rover to take a suboptimal path (red). The rover will now instead directly target the previous waypoint if the closest point on the path is not on the actual line segment leading to the more direct path (yellow).
In the same fashion, if the closest point on the path is ahead of the line segment the rover will target the current waypoint instead.
Logging message
This PR also adds a message for the purePursuit library to log relevant values.
This makes tuning of the pure pursuit algorithm easier and also deprecates the 3
[rover_type]_guidance_status.msgmessages.To use the library and publish the pure pursuit message the function 'updatePurePursuit' has to be called.
If the logging is not required then the function
calcTargetBearing(renamed fromcalcDesiredHeading) can also be called direclty, which will return the target bearing without publishing the pure pursuit message.Test coverage