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

@@ -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
    */

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

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