From 79137b06d273f1b01af0c1b56c0c6ac894e1af4c Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 16 Jan 2024 13:18:21 -0700 Subject: [PATCH 1/7] Added ROS2 dependencies to the path path_planner. --- rosplane/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt index 1617f18..0970d6a 100644 --- a/rosplane/CMakeLists.txt +++ b/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}) From 17f23988afc32269fdd1b17a7b2766a75ed24c36 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 16 Jan 2024 13:26:24 -0700 Subject: [PATCH 2/7] Added the max_roll parameter to rosplane. --- rosplane/include/controller_base.hpp | 4 +++- rosplane/params/succesive_loop_controller_params.yaml | 1 + rosplane/src/controller_base.cpp | 3 +++ rosplane/src/controller_successive_loop.cpp | 2 +- 4 files changed, 8 insertions(+), 2 deletions(-) diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 6859331..70ca413 100644 --- a/rosplane/include/controller_base.hpp +++ b/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 }; /** diff --git a/rosplane/params/succesive_loop_controller_params.yaml b/rosplane/params/succesive_loop_controller_params.yaml index 998d7e8..0b5abb6 100644 --- a/rosplane/params/succesive_loop_controller_params.yaml +++ b/rosplane/params/succesive_loop_controller_params.yaml @@ -39,3 +39,4 @@ autopilot: # node name. max_takeoff_throttle: 0.55 mass: 2.28 gravity: 9.8 + max_roll: 25.0 diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index d32e3b2..f7e72b0 100644 --- a/rosplane/src/controller_base.cpp +++ b/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. diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index 56da8ec..b8c91de 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/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; From dfa9af569e74c2680958ff00a3310d39192cf6ec Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 16 Jan 2024 13:32:10 -0700 Subject: [PATCH 3/7] Added heading (psi) to the path_follower inputs. --- rosplane/include/path_follower_base.hpp | 1 + rosplane/src/path_follower_base.cpp | 47 +++++++++++++------------ 2 files changed, 25 insertions(+), 23 deletions(-) diff --git a/rosplane/include/path_follower_base.hpp b/rosplane/include/path_follower_base.hpp index 9e0b547..7bd18a8 100644 --- a/rosplane/include/path_follower_base.hpp +++ b/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 diff --git a/rosplane/src/path_follower_base.cpp b/rosplane/src/path_follower_base.cpp index b2a8791..8a18d9a 100644 --- a/rosplane/src/path_follower_base.cpp +++ b/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 ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = false; - result.reason = ""; - - for (const auto ¶m : 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 ¶meters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = ""; + for (const auto ¶m : 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) From 138080b9eadb326f513f7f9f5ddb977add7de984 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 16 Jan 2024 13:38:47 -0700 Subject: [PATCH 4/7] Added parameter for obiting last waypoint. - Added the orbit_last parameter and added the path manager to the succesive_loop_controller_params file, and updated launch file to use the param file for the path_manager. --- rosplane/include/path_manager_base.hpp | 6 +++- rosplane/launch/rosplane.launch.py | 1 + .../succesive_loop_controller_params.yaml | 9 ++++-- rosplane/src/path_manager_base.cpp | 32 ++++++++++++++++++- 4 files changed, 44 insertions(+), 4 deletions(-) diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp index 5b584f1..4f841e4 100644 --- a/rosplane/include/path_manager_base.hpp +++ b/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 ¶ms, 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 ¶meters); }; } //end namespace #endif // PATH_MANAGER_BASE_H diff --git a/rosplane/launch/rosplane.launch.py b/rosplane/launch/rosplane.launch.py index 17efb3c..d957822 100644 --- a/rosplane/launch/rosplane.launch.py +++ b/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', diff --git a/rosplane/params/succesive_loop_controller_params.yaml b/rosplane/params/succesive_loop_controller_params.yaml index 0b5abb6..cd06d27 100644 --- a/rosplane/params/succesive_loop_controller_params.yaml +++ b/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 @@ -40,3 +40,8 @@ autopilot: # node name. mass: 2.28 gravity: 9.8 max_roll: 25.0 + +path_manager: + ros__parameters: + R_min: 100.0 + orbit_last: True diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp index 14f4e99..cc9091d 100755 --- a/rosplane/src/path_manager_base.cpp +++ b/rosplane/src/path_manager_base.cpp @@ -2,6 +2,7 @@ #include "path_manager_example.hpp" #include "rclcpp/rclcpp.hpp" #include "iostream" +#include 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 ¶meters) { + 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 ¶m : 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()); return 0; -} \ No newline at end of file +} From 3e2bfab958505dcb9d8c4312eb82f9169f48ae17 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 16 Jan 2024 13:41:39 -0700 Subject: [PATCH 5/7] Updated the feedforward term for roll to account for wind. --- rosplane/src/path_follower_example.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rosplane/src/path_follower_example.cpp b/rosplane/src/path_follower_example.cpp index 3564ff6..5c18d79 100644 --- a/rosplane/src/path_follower_example.cpp +++ b/rosplane/src/path_follower_example.cpp @@ -1,4 +1,5 @@ #include "path_follower_example.hpp" +#include namespace rosplane @@ -54,7 +55,8 @@ void path_follower_example::follow(const params_s ¶ms, 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; } From 61cc59fa79defff219581ef53f1df66da15de56a Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 16 Jan 2024 13:46:00 -0700 Subject: [PATCH 6/7] Implmented orbit last functionality. --- rosplane/src/path_manager_example.cpp | 77 +++++++++++++++++++++++++-- 1 file changed, 74 insertions(+), 3 deletions(-) diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp index 1c4ecb8..f1d30d7 100755 --- a/rosplane/src/path_manager_example.cpp +++ b/rosplane/src/path_manager_example.cpp @@ -3,6 +3,8 @@ #include "path_manager_example.hpp" #include "rclcpp/rclcpp.hpp" #include +#include +#include namespace rosplane { @@ -53,6 +55,34 @@ void path_manager_example::manage_line(const params_s ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 { From b8a16eec46526f8dea54646718912560fdba1a6d Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 16 Jan 2024 13:49:33 -0700 Subject: [PATCH 7/7] Implmented path_planner as an initial ROS2 node. - Added functionality to publish set of waypoints one at a time. --- rosplane/src/path_planner.cpp | 143 ++++++++++++++++++++++++++++------ 1 file changed, 121 insertions(+), 22 deletions(-) diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index e7098fc..7042bf5 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -1,41 +1,98 @@ #include "rclcpp/rclcpp.hpp" #include "rosplane_msgs/msg/waypoint.hpp" +#include +#include +#include +#include +#include +#include -#define num_waypoints 3 +#define num_waypoints 7 +#define num_waypoints_to_publish 3 -int main(int argc, char **argv) +using std::placeholders::_1; +using namespace std::chrono_literals; + +namespace rosplane { + +class path_planner : public rclcpp::Node { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("rosplane_simple_path_planner"); +public: + path_planner(); + ~path_planner(); + +private: + rclcpp::Publisher::SharedPtr waypoint_publisher; + rclcpp::Service::SharedPtr waypoint_service; + rclcpp::TimerBase::SharedPtr timer_; + + bool publish_next_waypoint( + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res); + + void waypoint_publish(); + + int num_waypoints_published; + + float wps[5*num_waypoints]; + +}; + +path_planner::path_planner() : Node("path_planner") { - auto waypoint_publisher = node->create_publisher("waypoint_path", 10); // !!! n vs nh_ example I am following used n - rclcpp::Rate loop_rate(10); - float va = 20; - float wps[5*num_waypoints] = + waypoint_publisher = this->create_publisher("waypoint_path", 10); + + waypoint_service = this->create_service("publish_next_waypoint", std::bind(&path_planner::publish_next_waypoint, this, std::placeholders::_1, std::placeholders::_2)); + + timer_ = this->create_wall_timer(1000ms, std::bind(&path_planner::waypoint_publish, this)); + + num_waypoints_published = 0; + + float va = 25; + float wps_to_add[5*num_waypoints] = { - 200, 0, -30, 45*M_PI/180, va, - 0, 200, -30, 45*M_PI/180, va, - 200, 200, -30, 225*M_PI/180, va, + 0, 100, -90, 45*M_PI/180, va, + 0, 300, -90, 45*M_PI/180, va, + 0, 500, -90, 45*M_PI/180, va, + 0, 700, -90, 45*M_PI/180, va, + 0, 900, -90, 45*M_PI/180, va, + 0, 1100, -90, 45*M_PI/180, va, + 0, 1300, -90, 45*M_PI/180, va }; - rclcpp::Clock wait; - for (int i(0); i < num_waypoints; i++) + for (int i = 0; i < 5*num_waypoints; i++) { + wps[i] = wps_to_add[i]; + } + +} + +path_planner::~path_planner() { +} + +bool path_planner::publish_next_waypoint( + const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ - wait.sleep_for(rclcpp::Duration(0,int(5e8))); + RCLCPP_INFO_STREAM(this->get_logger(), "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published); + if (num_waypoints_published < num_waypoints) + { rosplane_msgs::msg::Waypoint new_waypoint; - rclcpp::Time now = node->get_clock()->now(); + rclcpp::Time now = this->get_clock()->now(); new_waypoint.header.stamp = now; + int i = num_waypoints_published; + new_waypoint.w[0] = wps[i*5 + 0]; new_waypoint.w[1] = wps[i*5 + 1]; new_waypoint.w[2] = wps[i*5 + 2]; new_waypoint.chi_d = wps[i*5 + 3]; - new_waypoint.chi_valid = true; + new_waypoint.chi_valid = false; new_waypoint.va_d = wps[i*5 + 4]; if (i == 0) new_waypoint.set_current = true; @@ -43,17 +100,59 @@ int main(int argc, char **argv) new_waypoint.set_current = false; new_waypoint.clear_wp_list = false; - RCLCPP_DEBUG_STREAM(node->get_logger(), "Publishing Waypoint!"); + waypoint_publisher->publish(new_waypoint); + + num_waypoints_published++; + + } + + return true; +} + +void path_planner::waypoint_publish() +{ + + if (num_waypoints_published < num_waypoints_to_publish) + { + + rosplane_msgs::msg::Waypoint new_waypoint; - RCLCPP_DEBUG_STREAM(node->get_logger(), "North: " << new_waypoint.w[0]); - RCLCPP_DEBUG_STREAM(node->get_logger(), "East: " << new_waypoint.w[1]); - RCLCPP_DEBUG_STREAM(node->get_logger(), "Down: " << new_waypoint.w[2]); - RCLCPP_DEBUG_STREAM(node->get_logger(), "chi_d: " << new_waypoint.chi_d); + rclcpp::Time now = this->get_clock()->now(); + + new_waypoint.header.stamp = now; + + int i = num_waypoints_published; + + new_waypoint.w[0] = wps[i*5 + 0]; + new_waypoint.w[1] = wps[i*5 + 1]; + new_waypoint.w[2] = wps[i*5 + 2]; + new_waypoint.chi_d = wps[i*5 + 3]; + new_waypoint.chi_valid = false; + new_waypoint.va_d = wps[i*5 + 4]; + if (i == 0) + new_waypoint.set_current = true; + else + new_waypoint.set_current = false; + new_waypoint.clear_wp_list = false; waypoint_publisher->publish(new_waypoint); + + num_waypoints_published++; + } - wait.sleep_for(rclcpp::Duration(1,int(5e8))); + +} +} + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node); + return 0; }