diff --git a/src/include/fl/mathematics/control/PurePursuitTracker.h b/src/include/fl/mathematics/control/PurePursuitTracker.h index 876972a..521697a 100644 --- a/src/include/fl/mathematics/control/PurePursuitTracker.h +++ b/src/include/fl/mathematics/control/PurePursuitTracker.h @@ -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(); diff --git a/src/include/fl/mathematics/trajectory/TrajectoryIterator.h b/src/include/fl/mathematics/trajectory/TrajectoryIterator.h index 23a8015..3bea88c 100644 --- a/src/include/fl/mathematics/trajectory/TrajectoryIterator.h +++ b/src/include/fl/mathematics/trajectory/TrajectoryIterator.h @@ -3,6 +3,8 @@ #include "Trajectory.h" #include "fl/Utilities.h" +#include + namespace fl { template class TrajectoryIterator { @@ -32,7 +34,10 @@ class TrajectoryIterator { bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); } TrajectorySamplePoint 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: