Merge pull request #6 from rosflight/waypoint_following

Waypoint Following and Cycle Behavior

rosplane/CMakeLists.txt

@@ -79,7 +79,7 @@ install(TARGETS
 # Planner
 add_executable(rosplane_path_planner
             src/path_planner.cpp)
-ament_target_dependencies(rosplane_path_planner rosplane_msgs rclcpp rclpy Eigen3)
+ament_target_dependencies(rosplane_path_planner rosplane_msgs std_srvs rclcpp rclpy Eigen3)
 install(TARGETS
   rosplane_path_planner
   DESTINATION lib/${PROJECT_NAME})

rosplane/include/controller_base.hpp

@@ -130,6 +130,7 @@ class controller_base : public rclcpp::Node
     double gravity;              /**< gravity in m/s^2 */
     bool pitch_tuning_debug_override;
     bool roll_tuning_debug_override;
+    double max_roll;
   };
 
   /**
@@ -215,7 +216,8 @@ class controller_base : public rclcpp::Node
           /* mass */ 2.28,
           /* gravity */ 9.8,
           /* pitch_tuning_debug_override*/ false,
-          /* roll_tuning_debug_override*/ false
+          /* roll_tuning_debug_override*/ false,
+          /* max_roll*/ 25.0
     };
 
   /**

rosplane/include/path_follower_base.hpp

@@ -40,6 +40,7 @@ class path_follower_base : public rclcpp::Node
     float h;                /** altitude */
     float Va;               /** airspeed */
     float chi;              /** course angle */
+    float psi;              /** heading angle */
   };
 
   struct output_s

rosplane/include/path_manager_base.hpp

@@ -67,6 +67,7 @@ class path_manager_base : public rclcpp::Node
   struct params_s
   {
     double R_min;
+    bool orbit_last;
   };
 
   virtual void manage(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
@@ -87,7 +88,10 @@ class path_manager_base : public rclcpp::Node
   void vehicle_state_callback(const rosplane_msgs::msg::State &msg);
   bool state_init_;
   void new_waypoint_callback(const rosplane_msgs::msg::Waypoint &msg);
-  void current_path_publish(); 
+  void current_path_publish();
+
+  OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
+  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters);
 };
 } //end namespace
 #endif // PATH_MANAGER_BASE_H

rosplane/launch/rosplane.launch.py

@@ -39,6 +39,7 @@ def generate_launch_description():
             package='rosplane',
             executable='rosplane_path_manager',
             name='path_manager',
+            parameters = [autopilot_params],
         ),
         Node(
             package='rosplane',

rosplane/params/succesive_loop_controller_params.yaml

@@ -3,9 +3,9 @@ autopilot: # node name.
     alt_hz: 10.0
     alt_toz: 5.0
     tau: 50.0
-    c_kp: 2.37
+    c_kp: 3.5
     c_kd: 0.0
-    c_ki: 0.4
+    c_ki: 0.1
     r_kp: 0.06
     r_kd: 0.04
     r_ki: 0.0
@@ -39,3 +39,9 @@ autopilot: # node name.
     max_takeoff_throttle: 0.55
     mass: 2.28
     gravity: 9.8
+    max_roll: 25.0
+
+path_manager:
+  ros__parameters:
+    R_min: 100.0
+    orbit_last: True

rosplane/src/controller_base.cpp

@@ -196,6 +196,7 @@ void controller_base::actuator_controls_publish()
       this->declare_parameter("gravity", params_.gravity);
       this->declare_parameter("pitch_tuning_debug_override", params_.pitch_tuning_debug_override);
       this->declare_parameter("roll_tuning_debug_override", params_.roll_tuning_debug_override);
+      this->declare_parameter("max_roll", params_.max_roll);
     }
 
 void controller_base::set_parameters() {
@@ -243,6 +244,7 @@ void controller_base::set_parameters() {
   params_.gravity = this->get_parameter("gravity").as_double();
   params_.roll_tuning_debug_override = this->get_parameter("roll_tuning_debug_override").as_bool();
   params_.pitch_tuning_debug_override = this->get_parameter("pitch_tuning_debug_override").as_bool();
+  params_.max_roll = this->get_parameter("max_roll").as_double();
 
 }
 
@@ -295,6 +297,7 @@ rcl_interfaces::msg::SetParametersResult controller_base::parametersCallback(con
     else if (param.get_name() == "gravity") params_.gravity = param.as_double();
     else if (param.get_name() == "roll_tuning_debug_override") params_.roll_tuning_debug_override = param.as_bool();
     else if (param.get_name() == "pitch_tuning_debug_override") params_.pitch_tuning_debug_override = param.as_bool();
+    else if (param.get_name() == "max_roll") params_.max_roll = param.as_double();
     else{
 
       // If the parameter given doesn't match any of the parameters return false.

rosplane/src/controller_successive_loop.cpp

@@ -174,7 +174,7 @@ float controller_successive_loop::course_hold(float chi_c, float chi, float phi_
   float ui = params.c_ki*c_integrator_;
   float ud = params.c_kd*r;
 
-  float phi_c = sat(up + ui + ud + phi_ff, 15.0*3.14/180.0, -15.0*3.14/180.0);
+  float phi_c = sat(up + ui + ud + phi_ff, params.max_roll*3.14/180.0, -params.max_roll*3.14/180.0);
   if (fabs(params.c_ki) >= 0.00001)
   {
     float phi_c_unsat = up + ui + ud + phi_ff;

rosplane/src/path_follower_base.cpp

@@ -68,6 +68,7 @@ void path_follower_base::vehicle_state_callback(const rosplane_msgs::msg::State:
   input_.pe = msg->position[1];               /** position east */
   input_.h = -msg->position[2];                /** altitude */
   input_.chi = msg->chi;
+  input_.psi = msg->psi;
   input_.Va = msg->va;
 
   RCLCPP_DEBUG_STREAM(this->get_logger(), "FROM STATE -- input.chi: " << input_.chi);
@@ -95,34 +96,34 @@ void path_follower_base::current_path_callback(const rosplane_msgs::msg::Current
   current_path_init_ = true;
 }
 
-    rcl_interfaces::msg::SetParametersResult path_follower_base::parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
-        rcl_interfaces::msg::SetParametersResult result;
-        result.successful = false;
-        result.reason = "";
-
-        for (const auto &param : parameters) {
-
-            if (param.get_name() == "CHI_INFTY"){
-                params_.chi_infty = param.as_double();
-                result.successful = true;
-                result.reason = "success";
-            }
-            else if (param.get_name() == "K_PATH"){
-                params_.k_path = param.as_double();
-                result.successful = true;
-                result.reason = "success";
-            }
-            else if (param.get_name() == "K_ORBIT"){
-                params_.k_orbit = param.as_double();
-                result.successful = true;
-                result.reason = "success";
-            }
+rcl_interfaces::msg::SetParametersResult path_follower_base::parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = false;
+    result.reason = "";
 
+    for (const auto &param : parameters) {
+
+        if (param.get_name() == "CHI_INFTY"){
+            params_.chi_infty = param.as_double();
+            result.successful = true;
+            result.reason = "success";
+        }
+        else if (param.get_name() == "K_PATH"){
+            params_.k_path = param.as_double();
+            result.successful = true;
+            result.reason = "success";
+        }
+        else if (param.get_name() == "K_ORBIT"){
+            params_.k_orbit = param.as_double();
+            result.successful = true;
+            result.reason = "success";
         }
 
-        return result;
     }
 
+    return result;
+}
+
 } //end namespace
 
 int main(int argc, char **argv)

rosplane/src/path_follower_example.cpp

@@ -1,4 +1,5 @@
 #include "path_follower_example.hpp"
+#include <rclcpp/logging.hpp>
 
 
 namespace rosplane
@@ -54,7 +55,8 @@ void path_follower_example::follow(const params_s &params, const input_s &input,
     // commanded altitude is the height of the orbit
     float h_d = -input.c_orbit[2];
     output.h_c = h_d;
-    output.phi_ff = (norm_orbit_error < 0.5 ? input.lam_orbit*atanf(input.Va*input.Va/(9.8*input.rho_orbit)) : 0);
+    output.phi_ff = input.lam_orbit * std::atan(pow(input.Va, 2)/(9.81*input.rho_orbit*std::cos(input.chi - input.psi)));
+
   }
   output.Va_c = input.Va_d;
 }

rosplane/src/path_manager_base.cpp

@@ -2,6 +2,7 @@
 #include "path_manager_example.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "iostream"
+#include <rclcpp/logging.hpp>
 
 
 namespace rosplane
@@ -15,16 +16,45 @@ path_manager_base::path_manager_base() : Node("rosplane_path_manager")
   update_timer_      = this->create_wall_timer(10ms, std::bind(&path_manager_base::current_path_publish, this));
   // interesting read on wall timer
   // https://answers.ros.org/question/375561/create-wall-timer-using-callback-with-parameters-ros2-c/
+  //
+  // Set the parameter callback, for when parameters are changed.
+  parameter_callback_handle_ = this->add_on_set_parameters_callback(
+            std::bind(&path_manager_base::parametersCallback, this, std::placeholders::_1));
 
   this->declare_parameter("R_min", 25.0);
+  this->declare_parameter("orbit_last", false);
 
   params_.R_min = this->get_parameter("R_min").as_double();
+  params_.orbit_last = this->get_parameter("orbit_last").as_bool();
 
   num_waypoints_ = 0;
 
   state_init_ = false;
 }
 
+rcl_interfaces::msg::SetParametersResult path_manager_base::parametersCallback(const std::vector<rclcpp::Parameter> &parameters) {
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  result.reason = "success";
+
+  // Check each parameter in the incoming vector of parameters to change, and change the appropriate parameter.
+  for (const auto &param : parameters) {
+
+    if (param.get_name() == "R_min") params_.R_min = param.as_double();
+    else if (param.get_name() == "orbit_last") params_.orbit_last = param.as_bool();
+    else{
+
+      // If the parameter given doesn't match any of the parameters return false.
+      result.successful = false;
+      result.reason = "One of the parameters given does not is not a parameter of the controller node.";
+      break;
+    }
+
+  }
+
+  return result;
+}
+
 void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State &msg)
 {
 
@@ -131,4 +161,4 @@ int main(int argc, char **argv)
   rclcpp::spin(std::make_shared<rosplane::path_manager_example>());
 
   return 0;
-}
+}

rosplane/src/path_manager_example.cpp

@@ -3,6 +3,8 @@
 #include "path_manager_example.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include <cmath>
+#include <rclcpp/logging.hpp>
+#include <string>
 
 namespace rosplane
 {
@@ -53,6 +55,34 @@ void path_manager_example::manage_line(const params_s &params, const input_s &in
   int idx_c;
   if (idx_a_ == num_waypoints_ - 1)
   {
+
+    if (params.orbit_last)
+    {
+      output.flag = false;
+      output.va_d = waypoints_[idx_a_].va_d;
+      output.c[0] = waypoints_[idx_a_].w[0]; 
+      output.c[1] = waypoints_[idx_a_].w[1];
+      output.c[2] = waypoints_[idx_a_].w[2];
+      output.r[0] = 0.0;
+      output.r[1] = 0.0;
+      output.r[2] = 0.0;
+      output.q[0] = 0.0;
+      output.q[1] = 0.0;
+      output.q[2] = 0.0;
+      output.rho = params.R_min;
+
+      if (waypoints_[idx_a_].chi_d < M_PI)
+      {
+        output.lamda = 1; 
+      }
+      else 
+      {
+        output.lamda = -1;
+      }
+
+      return;
+    }
+
     idx_b = 0;
     idx_c = 1;
   }
@@ -64,8 +94,7 @@ void path_manager_example::manage_line(const params_s &params, const input_s &in
   else
   {
     idx_b = idx_a_ + 1;
-    idx_c = idx_b + 1;
-  }
+    idx_c = idx_b + 1; }
 
   Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
   Eigen::Vector3f w_i(waypoints_[idx_b].w);
@@ -86,9 +115,13 @@ void path_manager_example::manage_line(const params_s &params, const input_s &in
   if ((p - w_i).dot(n_i) > 0.0f)
   {
     if (idx_a_ == num_waypoints_ - 1)
+    {
       idx_a_ = 0;
+    }
     else
+    {
       idx_a_++;
+    }
   }
 
 }
@@ -108,13 +141,51 @@ void path_manager_example::manage_fillet(const params_s &params, const input_s &
   int idx_c;
   if (idx_a_ == num_waypoints_ - 1)
   {
+
+    if (params.orbit_last)
+    {
+      output.flag = false;
+      output.va_d = waypoints_[idx_a_].va_d;
+      output.c[0] = waypoints_[idx_a_].w[0]; 
+      output.c[1] = waypoints_[idx_a_].w[1];
+      output.c[2] = waypoints_[idx_a_].w[2];
+      output.r[0] = 0.0;
+      output.r[1] = 0.0;
+      output.r[2] = 0.0;
+      output.q[0] = 0.0;
+      output.q[1] = 0.0;
+      output.q[2] = 0.0;
+      output.rho = params.R_min;
+
+      if (waypoints_[idx_a_].chi_d < M_PI)
+      {
+        output.lamda = 1; 
+      }
+      else 
+      {
+        output.lamda = -1;
+      }
+
+      return;
+    }
+
     idx_b = 0;
     idx_c = 1;
   }
   else if (idx_a_ == num_waypoints_ - 2)
   {
     idx_b = num_waypoints_ - 1;
-    idx_c = 0;
+
+    if (params.orbit_last)
+    {
+      idx_c = 0;
+      idx_a_ = idx_b;
+      return;
+    }
+    else 
+    {
+      idx_c = 0;
+    }
   }
   else
   {