Fixed the param manager issues for setting temporary waypoints.

rosplane/src/path_manager_base.cpp

@@ -39,6 +39,8 @@ path_manager_base::path_manager_base()
 void path_manager_base::declare_parameters() {
   params_.declare_double("R_min", 50.0);
   params_.declare_double("current_path_pub_frequency", 100.0);
+  params_.declare_double("default_altitude", 50.0);
+  params_.declare_double("default_airspeed", 15.0);
 }
 
 void path_manager_base::set_timer() {
@@ -89,6 +91,7 @@ void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State &
 void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg)
 {
   double R_min = params_.get_double("R_min");
+  double default_altitude = params_.get_double("default_altitude");
   orbit_dir = 0;
 
   // If the message contains "clear_wp_list", then clear all waypoints and do nothing else
@@ -107,7 +110,14 @@ void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint
 
     temp_waypoint.w[0] = vehicle_state_.position[0];
     temp_waypoint.w[1] = vehicle_state_.position[1];
-    temp_waypoint.w[2] = vehicle_state_.position[2];
+
+    if (vehicle_state_.position[2] < -default_altitude) {
+
+      temp_waypoint.w[2] = vehicle_state_.position[2];
+    }
+    else {
+      temp_waypoint.w[2] = -default_altitude;
+    }
 
     temp_waypoint.chi_d = 0.0; // Doesn't matter, it is never used.
     temp_waypoint.use_chi = false;

rosplane/src/path_manager_example.cpp

@@ -40,6 +40,12 @@ void path_manager_example::manage(const input_s & input, output_s & output)
       RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No waypoints received, orbiting origin at " << default_altitude << " meters.");
       output.flag = false; // Indicate that the path is an orbit.
       output.va_d = default_airspeed; // Set to the default_airspeed.
+      output.q[0] = 0.0f; // initialize the parameters to have a value.
+      output.q[1] = 0.0f;
+      output.q[2] = 0.0f;
+      output.r[0] = 0.0f; // initialize the parameters to have a value.
+      output.r[1] = 0.0f;
+      output.r[2] = 0.0f;
       output.c[0] = 0.0f; // Direcct the center of the orbit to the origin at the default default_altitude.
       output.c[1] = 0.0f;
       output.c[2] = -default_altitude;
@@ -52,6 +58,12 @@ void path_manager_example::manage(const input_s & input, output_s & output)
     // If only a single waypoint is given, orbit it.
     output.flag = false;
     output.va_d = waypoints_[0].va_d;
+    output.q[0] = 0.0f; // initialize the parameters to have a value.
+    output.q[1] = 0.0f;
+    output.q[2] = 0.0f;
+    output.r[0] = 0.0f; // initialize the parameters to have a value.
+    output.r[1] = 0.0f;
+    output.r[2] = 0.0f;
     output.c[0] = waypoints_[0].w[0];
     output.c[1] = waypoints_[0].w[1];
     output.c[2] = waypoints_[0].w[2];
@@ -145,6 +157,7 @@ void path_manager_example::manage_fillet(const input_s & input,
 
   if (orbit_last && idx_a_ == num_waypoints_ - 1)
   {
+    // TODO: check to see if this is the correct behavior.
     return;
   }
 
@@ -563,8 +576,6 @@ void path_manager_example::dubinsParameters(const waypoint_s start_node, const w
 void path_manager_example::declare_parameters()
 {
   params_.declare_bool("orbit_last", false);
-  params_.declare_double("default_altitude", 50.0);
-  params_.declare_double("default_airspeed", 15.0);
 }
 
 int path_manager_example::orbit_direction(float pn, float pe, float chi, float c_n, float c_e)