prateekma/FalconLibraryCPP
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fix Pure Pursuit bug

Browse Source
This commit is contained in:
Prateek Machiraju 2019-06-27 09:45:23 -04:00
parent bd58f94e9a
commit f2c45f5cd5
2 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/include/fl/mathematics/control/PurePursuitTracker.h
View File

@ -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();

5
src/include/fl/mathematics/trajectory/TrajectoryIterator.h
View File

@ -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 {
@ -33,6 +35,9 @@ class TrajectoryIterator {
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: