From ddd2db5f027bf349d37f12f16abe9687370c415f Mon Sep 17 00:00:00 2001
From: Ian Reid <ian.young.reid@gmail.com>
Date: Mon, 6 Jan 2025 09:43:02 -0700
Subject: [PATCH] Fixed the dubins path following and the output is initialized
 correctly.

The dubins path was always going back to 0,0. This was undesirable.
Also, because the output struct was not initialized, when plotting
values in the struct, they would start at a random, usually very large,
value.
---
 rosplane/include/path_manager_example.hpp |  2 ++
 rosplane/src/path_manager_base.cpp        | 10 ++++++++++
 rosplane/src/path_manager_example.cpp     | 11 ++++++++++-
 3 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/rosplane/include/path_manager_example.hpp b/rosplane/include/path_manager_example.hpp
index d254414..ec026cf 100644
--- a/rosplane/include/path_manager_example.hpp
+++ b/rosplane/include/path_manager_example.hpp
@@ -37,6 +37,8 @@ class PathManagerExample : public PathManagerBase
   std::chrono::time_point<std::chrono::system_clock> start_time_;
   FilletState fil_state_;
 
+  bool first_;
+
   /**
    * @brief Determines the line type and calculates the line parameters to publish to path_follower
    */
diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp
index 3b61fb4..6f87da4 100644
--- a/rosplane/src/path_manager_base.cpp
+++ b/rosplane/src/path_manager_base.cpp
@@ -172,6 +172,16 @@ void PathManagerBase::current_path_publish()
   input.chi = vehicle_state_.chi;
 
   Output output;
+  output.va_d = 0;
+  output.r[0] = 0;
+  output.r[1] = 0;
+  output.r[2] = 0;
+  output.q[0] = 0;
+  output.q[1] = 0;
+  output.q[2] = 0;
+  output.c[0] = 0;
+  output.c[1] = 0;
+  output.c[2] = 0;
 
   if (state_init_ == true) {
     manage(input, output);
diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp
index aea08a8..9446e1b 100644
--- a/rosplane/src/path_manager_example.cpp
+++ b/rosplane/src/path_manager_example.cpp
@@ -20,6 +20,8 @@ PathManagerExample::PathManagerExample()
   params_.set_parameters();
 
   start_time_ = std::chrono::system_clock::now();
+
+  first_ = true;
 }
 
 void PathManagerExample::manage(const Input & input, Output & output)
@@ -154,7 +156,6 @@ void PathManagerExample::manage_fillet(const Input & input, Output & output)
   increment_indices(idx_a_, idx_b, idx_c, input, output);
 
   if (orbit_last && idx_a_ == num_waypoints_ - 1) {
-    // TODO: check to see if this is the correct behavior.
     return;
   }
 
@@ -399,6 +400,14 @@ void PathManagerExample::manage_dubins(const Input & input, Output & output)
         } else {
           idx_a_++;
           idx_b = idx_a_ + 1;
+
+          if (first_) {
+            first_ = false;
+            waypoints_.erase(waypoints_.begin());
+            num_waypoints_--;
+            idx_a_--;
+            idx_b = idx_a_ + 1;
+          }
         }
 
         // plan new Dubin's path to next waypoint configuration