Fix Pure Pursuit bug
This commit is contained in:
parent
bd58f94e9a
commit
f2c45f5cd5
|
@ -60,7 +60,7 @@ class PurePursuitTracker : public TrajectoryTracker {
|
|||
|
||||
// Run the loop until a distance that is greater than the minimum lookahead distance is found or until
|
||||
// we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory.
|
||||
while (iterator.Progress() > previewed_time) {
|
||||
while (iterator.RemainingProgress() > previewed_time) {
|
||||
previewed_time += 0.02;
|
||||
lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
|
||||
|
||||
|
|
|
@ -3,6 +3,8 @@
|
|||
#include "Trajectory.h"
|
||||
#include "fl/Utilities.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace fl {
|
||||
template <typename U, typename S>
|
||||
class TrajectoryIterator {
|
||||
|
@ -32,7 +34,10 @@ class TrajectoryIterator {
|
|||
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
|
||||
TrajectorySamplePoint<S> CurrentState() const { return sample_; }
|
||||
|
||||
U Progress() const { return progress_; }
|
||||
U Progress() const { return progress_; }
|
||||
U RemainingProgress() const {
|
||||
return std::max(trajectory_->FirstInterpolant(), trajectory_->LastInterpolant() - progress_);
|
||||
}
|
||||
bool Reversed() const { return trajectory_->Reversed(); }
|
||||
|
||||
protected:
|
||||
|
|
Loading…
Reference in New Issue
Block a user