fix trajectory tracking

src/bench/bench_trajectory_generation.cpp

@@ -1,6 +1,8 @@
 #include <FalconLibrary.h>
 #include <benchmark/benchmark.h>
 
+#include <vector>
+
 static void BM_TrajectoryGeneration(benchmark::State& state) {
   const fl::Pose2d initial{0.0, 0.0, fl::Rotation2d::FromDegrees(0.0)};
   const fl::Pose2d final{10.0, 10.0, fl::Rotation2d::FromDegrees(45.0)};

src/include/fl/mathematics/control/TrajectoryTracker.h

@@ -21,30 +21,35 @@ struct TrajectoryTrackerOutput {
 class TrajectoryTracker {
  public:
   void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
-    iterator_          = dynamic_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
+    iterator_          = static_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get());
     previous_velocity_ = nullptr;
     previous_time_     = -1.;
   }
 
   TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) {
     if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
-    const auto& iterator = *iterator_;
+    auto& iterator = *iterator_;
 
     const auto dt  = (previous_time_ < 0.0) ? 0.0 : current_time - previous_time_;
     previous_time_ = current_time;
 
-    const auto velocity          = CalculateState(iterator, current_pose);
-    const auto previous_velocity = *previous_velocity_;
+    iterator.Advance(dt);
+
+    const auto velocity = CalculateState(iterator, current_pose);
 
     const auto linear_velocity  = velocity.linear_velocity;
     const auto angular_velocity = velocity.angular_velocity;
 
+    if (previous_velocity_ == nullptr || dt <= 0.) {
+      previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity});
+      return {linear_velocity, 0.0, angular_velocity, 0.0};
+    }
+
+    const auto previous_velocity = *previous_velocity_;
+
     previous_velocity_->linear_velocity  = linear_velocity;
     previous_velocity_->angular_velocity = angular_velocity;
 
-    if (previous_velocity_ == nullptr || dt <= 0.) {
-      return {linear_velocity, 0.0, angular_velocity, 0.0};
-    }
     return {linear_velocity, (linear_velocity - previous_velocity.linear_velocity) / dt, angular_velocity,
             (angular_velocity - previous_velocity.angular_velocity) / dt};
   }

src/include/fl/mathematics/trajectory/Trajectory.h

@@ -2,6 +2,7 @@
 
 #include <utility>
 #include <vector>
+#include <memory>
 
 namespace fl {
 

src/test/cpp/ramsete-tests.cpp

@@ -1,10 +1,12 @@
 #include <gtest/gtest.h>
 #include "FalconLibrary.h"
 
+constexpr double kTestEpsilon = 1E-9;
+
 class RamseteTest : public ::testing::Test {
  public:
-  void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3,
-           double max_acceleration = 2, bool backwards = false) {
+  void Run(fl::Pose2d initial, fl::Pose2d final, double max_velocity = 3, double max_acceleration = 2,
+           bool backwards = false) {
     auto trajectory = fl::TrajectoryGenerator::GenerateTrajectory(
         std::vector<fl::Pose2d>{initial, final},
         std::vector<fl::TimingConstraint<fl::Pose2dWithCurvature>*>{}, 0.0, 0.0, max_velocity,
@@ -12,6 +14,22 @@ class RamseteTest : public ::testing::Test {
 
     fl::RamseteTracker tracker{2.0, 0.7};
     tracker.Reset(trajectory);
+
+    fl::Pose2d robot_pose = initial;
+    double     t          = 0.0;
+
+    while (!tracker.IsFinished()) {
+      const auto output = tracker.NextState(robot_pose, t);
+
+      fl::Twist2d twist{output.linear_velocity * 0.02, 0.0, output.angular_velocity * 0.02};
+      robot_pose = robot_pose + fl::Pose2d::FromTwist(twist);
+
+      t += 0.02;
+    }
+
+    EXPECT_NEAR(robot_pose.Translation().X(), final.Translation().X(), 0.1);
+    EXPECT_NEAR(robot_pose.Translation().Y(), final.Translation().Y(), 0.1);
+    EXPECT_NEAR(robot_pose.Rotation().Radians(), final.Rotation().Radians(), 0.1);
   }
 };