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
|
// 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.
|
// 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;
|
previewed_time += 0.02;
|
||||||
lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
|
lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose();
|
||||||
|
|
||||||
|
|
|
@ -3,6 +3,8 @@
|
||||||
#include "Trajectory.h"
|
#include "Trajectory.h"
|
||||||
#include "fl/Utilities.h"
|
#include "fl/Utilities.h"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
namespace fl {
|
namespace fl {
|
||||||
template <typename U, typename S>
|
template <typename U, typename S>
|
||||||
class TrajectoryIterator {
|
class TrajectoryIterator {
|
||||||
|
@ -32,7 +34,10 @@ class TrajectoryIterator {
|
||||||
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
|
bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
|
||||||
TrajectorySamplePoint<S> CurrentState() const { return sample_; }
|
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(); }
|
bool Reversed() const { return trajectory_->Reversed(); }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
Loading…
Reference in New Issue
Block a user