From 2c4790c40868fce3ef97a7dec5fe516bcfdcffa1 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Tue, 18 Jun 2024 08:38:31 -0600 Subject: [PATCH 1/8] Added doxygen to the path planner. Also refactored some functions for clarity --- rosplane/src/path_planner.cpp | 106 +++++++++++++++++++++++++++++----- 1 file changed, 90 insertions(+), 16 deletions(-) diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index 006d6ad..119ee06 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -7,7 +7,6 @@ #include #include #include -// #include #include #include #include @@ -38,31 +37,107 @@ class path_planner : public rclcpp::Node rclcpp::Service::SharedPtr print_waypoint_service; rclcpp::Service::SharedPtr add_waypoint_service; rclcpp::Service::SharedPtr load_mission_service; - rclcpp::TimerBase::SharedPtr timer_; + /** + * @brief "publish_next_waypoint" service callback. Publish the next waypoint from the internal vector of waypoint objects. Will not publish if there are no more waypoints in the vector. + * + * @param req: Pointer to a std_srvs::srv::Trigger request object + * @param res: Pointer to a std_srvs::srv::Trigger response object + * + * @return True if waypoint was published, false if no more waypoints were available to publish + */ bool publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res); + + /** + * @brief "add_waypoint" service callback. Adds a waypoint to the end of the vector of waypoint objects and optionally publishes another waypoint. + * + * @param req: Pointer to an AddWaypoint service request object + * @param req: Pointer to an AddWaypoint service response object + * + * @return True + */ bool update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req, const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res); + + /** + * @brief "clear_path" service callback. Clears all the waypoints internally and sends clear commands to path_manager + * + * @param req: Pointer to a Trigger service request object + * @param req: Pointer to a Trigger service response object + * + * @return True + */ bool clear_path_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res); void clear_path(); + + /** + * @brief "print_path" service callback. Prints waypoints to the terminal + * + * @param req: Pointer to a Trigger service request object + * @param req: Pointer to a Trigger service response object + * + * @return True + */ bool print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res); + + /** + * @brief "load_mission_from_file" service callback + * + * @param req: Pointer to a ParamFile service request object + * @param req: Pointer to a ParamFile service response object + * + * @return True + */ bool load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req, const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res); + + /** + * @brief Parses YAML file and loads waypoints + * + * @param filename: String containing the path to the YAML file + * + * @return True if loading waypoints was successful, false otherwise + */ bool load_mission_from_file(const std::string& filename); + + /** + * @brief Callback for the rosplane_msgs::msg::State publisher. Saves the initial GNSS coordinates + * + * @param msg: Pointer to a rosplane_msgs::msg::State object + */ void state_callback(const rosplane_msgs::msg::State & msg); + + /** + * @brief Publishes the next waypoint in the vector of waypoints + */ void waypoint_publish(); - void timer_callback(); + + /** + * @brief Publishes the number of initial waypoints given by parameter + */ + void publish_initial_waypoints(); + + /** + * @brief Converts an LLA coordinate to NED coordinates + * + * @param lla: Array of floats of size 3, with [latitude, longitude, altitude] + * @return Array of doubles corresponding to the NED coordinates measured from the origin + */ std::array lla2ned(std::array lla); /** - * This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. - * It also sets the default parameter, which will then be overridden by a launch script. + * @brief This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. It also sets the default parameter, which will then be overridden by a launch script. */ void declare_parameters(); OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + /** + * @brief Parameter change callback + * + * @param parameters: Vector of rclcpp::Parameter that have been changed. + */ rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector & parameters); @@ -78,6 +153,7 @@ path_planner::path_planner() : Node("path_planner"), params(this) { + // Make this publisher transient_local so that it publishes the last 10 waypoints to late subscribers rclcpp::QoS qos_transient_local_10_(10); qos_transient_local_10_.transient_local(); waypoint_publisher = this->create_publisher("waypoint_path", qos_transient_local_10_); @@ -103,33 +179,33 @@ path_planner::path_planner() // Set the parameter callback, for when parameters are changed. parameter_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&path_planner::parametersCallback, this, std::placeholders::_1)); - declare_parameters(); - params.set_parameters(); - timer_ = this->create_wall_timer(1000ms, std::bind(&path_planner::timer_callback, this)); + declare_parameters(); num_waypoints_published = 0; // Initialize by publishing a clear path command. - // This makes sure rviz doesn't show stale waypoints if rosplane is restarted. + // This makes sure rviz or other vizualization tools don't show stale waypoints if rosplane is restarted. clear_path(); - // Print out helpful information - RCLCPP_INFO_STREAM(this->get_logger(), "Path Planner will publish the first {" << this->get_parameter("num_waypoints_to_publish_at_start").as_int() << "} available waypoints!"); + // Publishes the initial waypoints + publish_initial_waypoints(); } path_planner::~path_planner() {} -void path_planner::timer_callback() { +void path_planner::publish_initial_waypoints() { int num_waypoints_to_publish_at_start = this->get_parameter("num_waypoints_to_publish_at_start").as_int(); - if (num_waypoints_published < num_waypoints_to_publish_at_start && num_waypoints_published < (int) wps.size()) { + RCLCPP_INFO_STREAM(this->get_logger(), "Path Planner will publish the first {" << num_waypoints_to_publish_at_start << "} available waypoints!"); + + while (num_waypoints_published < num_waypoints_to_publish_at_start && num_waypoints_published < (int) wps.size()) { waypoint_publish(); } } void path_planner::state_callback(const rosplane_msgs::msg::State & msg) { - // Make sure initial LLA is not zero + // Make sure initial LLA is not zero before updating to avoid errors if (fabs(msg.initial_lat) > 0.0 || fabs(msg.initial_lon) > 0.0 || fabs(msg.initial_alt) > 0.0) { initial_lat_ = msg.initial_lat; initial_lon_ = msg.initial_lon; @@ -140,7 +216,6 @@ void path_planner::state_callback(const rosplane_msgs::msg::State & msg) { bool path_planner::publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { - if (num_waypoints_published < (int) wps.size()) { RCLCPP_INFO_STREAM( this->get_logger(), @@ -160,7 +235,6 @@ bool path_planner::publish_next_waypoint(const std_srvs::srv::Trigger::Request:: void path_planner::waypoint_publish() { - rosplane_msgs::msg::Waypoint new_waypoint = wps[num_waypoints_published]; new_waypoint.clear_wp_list = false; From 85af9447fb6636398ca61f8b89224909090d51a6 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Tue, 18 Jun 2024 09:13:18 -0600 Subject: [PATCH 2/8] added set_parameters method call back in --- rosplane/src/path_planner.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index 119ee06..9e063ab 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -181,6 +181,7 @@ path_planner::path_planner() std::bind(&path_planner::parametersCallback, this, std::placeholders::_1)); declare_parameters(); + params.set_parameters(); num_waypoints_published = 0; From 5c79a8ddb0f44fafcf8d326be0f2e541257d140d Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Tue, 18 Jun 2024 12:53:34 -0600 Subject: [PATCH 3/8] slight name changes to align with standards as I was adding documentation --- rosplane/include/param_manager.hpp | 2 -- rosplane/src/controller_base.cpp | 2 +- rosplane/src/param_manager.cpp | 2 +- rosplane/src/path_manager_base.cpp | 2 +- rosplane/src/path_planner.cpp | 34 +++++++++++++++--------------- 5 files changed, 20 insertions(+), 22 deletions(-) diff --git a/rosplane/include/param_manager.hpp b/rosplane/include/param_manager.hpp index 5f7e62b..93b7645 100644 --- a/rosplane/include/param_manager.hpp +++ b/rosplane/include/param_manager.hpp @@ -47,8 +47,6 @@ class param_manager * This sets the parameters with the values in the params_ object from the supplied parameter file, or sets them to * the default if no value is given for a parameter. */ - // TODO: Check to make sure that setting a parameter before declaring it won't give an error. - // Hypothesis is that it will break, but is that not desired behavior? void set_parameters(); /** diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index 2b894f0..e6e6fe8 100644 --- a/rosplane/src/controller_base.cpp +++ b/rosplane/src/controller_base.cpp @@ -153,7 +153,7 @@ controller_base::parametersCallback(const std::vector & param { result.successful = false; result.reason = - "One of the parameters given does not is not a parameter of the controller node."; + "One of the parameters given is not a parameter of the controller node."; } return result; diff --git a/rosplane/src/param_manager.cpp b/rosplane/src/param_manager.cpp index add0950..093410e 100644 --- a/rosplane/src/param_manager.cpp +++ b/rosplane/src/param_manager.cpp @@ -117,7 +117,7 @@ bool param_manager::set_parameters_callback(const std::vector // Check if the parameter is in the params object or return an error if (params_.find(param.get_name()) == params_.end()) { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "One of the parameters given does not is not a parameter of the controller node. Parameter: " + param.get_name()); + RCLCPP_ERROR_STREAM(container_node_->get_logger(), "One of the parameters given is not a parameter of the controller node. Parameter: " + param.get_name()); return false; } diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp index aa25160..46ba34c 100644 --- a/rosplane/src/path_manager_base.cpp +++ b/rosplane/src/path_manager_base.cpp @@ -34,7 +34,7 @@ path_manager_base::parametersCallback(const std::vector & par { rcl_interfaces::msg::SetParametersResult result; result.successful = false; - result.reason = "One of the parameters given does not is not a parameter of the controller node."; + result.reason = "One of the parameters given is not a parameter of the controller node."; bool success = params.set_parameters_callback(parameters); if (success) diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index 9e063ab..8ecf304 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -30,13 +30,13 @@ class path_planner : public rclcpp::Node param_manager params; /** Holds the parameters for the path_planner*/ private: - rclcpp::Publisher::SharedPtr waypoint_publisher; - rclcpp::Subscription::SharedPtr state_subscription; - rclcpp::Service::SharedPtr next_waypoint_service; - rclcpp::Service::SharedPtr clear_waypoint_service; - rclcpp::Service::SharedPtr print_waypoint_service; - rclcpp::Service::SharedPtr add_waypoint_service; - rclcpp::Service::SharedPtr load_mission_service; + rclcpp::Publisher::SharedPtr waypoint_publisher_; + rclcpp::Subscription::SharedPtr state_subscription_; + rclcpp::Service::SharedPtr next_waypoint_service_; + rclcpp::Service::SharedPtr clear_waypoint_service_; + rclcpp::Service::SharedPtr print_waypoint_service_; + rclcpp::Service::SharedPtr add_waypoint_service_; + rclcpp::Service::SharedPtr load_mission_service_; /** * @brief "publish_next_waypoint" service callback. Publish the next waypoint from the internal vector of waypoint objects. Will not publish if there are no more waypoints in the vector. @@ -156,24 +156,24 @@ path_planner::path_planner() // Make this publisher transient_local so that it publishes the last 10 waypoints to late subscribers rclcpp::QoS qos_transient_local_10_(10); qos_transient_local_10_.transient_local(); - waypoint_publisher = this->create_publisher("waypoint_path", qos_transient_local_10_); + waypoint_publisher_ = this->create_publisher("waypoint_path", qos_transient_local_10_); - next_waypoint_service = this->create_service( + next_waypoint_service_ = this->create_service( "publish_next_waypoint", std::bind(&path_planner::publish_next_waypoint, this, _1, _2)); - add_waypoint_service = this->create_service( + add_waypoint_service_ = this->create_service( "add_waypoint", std::bind(&path_planner::update_path, this, _1, _2)); - clear_waypoint_service = this->create_service( + clear_waypoint_service_ = this->create_service( "clear_waypoints", std::bind(&path_planner::clear_path_callback, this, _1, _2)); - print_waypoint_service = this->create_service( + print_waypoint_service_ = this->create_service( "print_waypoints", std::bind(&path_planner::print_path, this, _1, _2)); - load_mission_service = this->create_service( + load_mission_service_ = this->create_service( "load_mission_from_file", std::bind(&path_planner::load_mission, this, _1, _2)); - state_subscription = this->create_subscription("estimated_state", 10, + state_subscription_ = this->create_subscription("estimated_state", 10, std::bind(&path_planner::state_callback, this, _1)); // Set the parameter callback, for when parameters are changed. @@ -239,7 +239,7 @@ void path_planner::waypoint_publish() rosplane_msgs::msg::Waypoint new_waypoint = wps[num_waypoints_published]; new_waypoint.clear_wp_list = false; - waypoint_publisher->publish(new_waypoint); + waypoint_publisher_->publish(new_waypoint); num_waypoints_published++; } @@ -300,7 +300,7 @@ void path_planner::clear_path() { rosplane_msgs::msg::Waypoint new_waypoint; new_waypoint.clear_wp_list = true; - waypoint_publisher->publish(new_waypoint); + waypoint_publisher_->publish(new_waypoint); num_waypoints_published = 0; } @@ -402,7 +402,7 @@ path_planner::parametersCallback(const std::vector & paramete { rcl_interfaces::msg::SetParametersResult result; result.successful = false; - result.reason = "One of the parameters given does not is not a parameter of the controller node."; + result.reason = "One of the parameters given is not a parameter of the controller node."; bool success = params.set_parameters_callback(parameters); if (success) From 7413ebcc20fd228a43b66d5a8ea1e0c7813c3d76 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 26 Jun 2024 15:07:22 -0600 Subject: [PATCH 4/8] updates to param_manager documentation --- rosplane/include/param_manager.hpp | 54 +++++++++++++++++++++++++++--- rosplane/src/param_manager.cpp | 3 +- 2 files changed, 51 insertions(+), 6 deletions(-) diff --git a/rosplane/include/param_manager.hpp b/rosplane/include/param_manager.hpp index a9b6634..1cdd270 100644 --- a/rosplane/include/param_manager.hpp +++ b/rosplane/include/param_manager.hpp @@ -1,7 +1,7 @@ /** * @file param_manager.hpp * - * Manager for the parameters in ROSplane. Includes helper functions to call parameters + * Manager for the parameters in ROSplane. Includes helper functions to set and get parameters * * @author Jacob Moore */ @@ -26,21 +26,51 @@ class param_manager param_manager(rclcpp::Node * node); /** - * Helper functions to access parameter values stored in param_manager object - * Returns a std::variant that holds the value of the given parameter + * Helper function to access parameter values of type double stored in param_manager object + * @return Double value of the parameter */ double get_double(std::string param_name); + + /** + * Helper function to access parameter values of type bool stored in param_manager object + * @return Bool value of the parameter + */ bool get_bool(std::string param_name); + + /** + * Helper function to access parameter values of type integer stored in param_manager object + * @return Integer value of the parameter + */ int64_t get_int(std::string param_name); + + /** + * Helper function to access parameter values of type string stored in param_manager object + * @return String value of the parameter + */ std::string get_string(std::string param_name); /** - * Helper functions to declare parameters in the param_manager object + * Helper function to declare parameters in the param_manager object * Inserts a parameter into the parameter object and declares it with the ROS system */ void declare_double(std::string param_name, double value); + + /** + * Helper function to declare parameters in the param_manager object + * Inserts a parameter into the parameter object and declares it with the ROS system + */ void declare_bool(std::string param_name, bool value); + + /** + * Helper function to declare parameters in the param_manager object + * Inserts a parameter into the parameter object and declares it with the ROS system + */ void declare_int(std::string param_name, int64_t value); + + /** + * Helper function to declare parameters in the param_manager object + * Inserts a parameter into the parameter object and declares it with the ROS system + */ void declare_string(std::string param_name, std::string value); /** @@ -54,8 +84,23 @@ class param_manager * and the ROS system. */ void set_double(std::string param_name, double value); + + /** + * This function sets a previously declared parameter to a new value in both the parameter object + * and the ROS system. + */ void set_bool(std::string param_name, bool value); + + /** + * This function sets a previously declared parameter to a new value in both the parameter object + * and the ROS system. + */ void set_int(std::string param_name, int64_t value); + + /** + * This function sets a previously declared parameter to a new value in both the parameter object + * and the ROS system. + */ void set_string(std::string param_name, std::string value); /** @@ -75,6 +120,5 @@ class param_manager rclcpp::Node * container_node_; }; - } // namespace rosplane #endif // PARAM_MANAGER_H diff --git a/rosplane/src/param_manager.cpp b/rosplane/src/param_manager.cpp index e41ecd4..b577dba 100644 --- a/rosplane/src/param_manager.cpp +++ b/rosplane/src/param_manager.cpp @@ -1,4 +1,5 @@ #include "param_manager.hpp" +#include namespace rosplane { @@ -153,7 +154,7 @@ void param_manager::set_parameters() { // Get the parameters from the launch file, if given. - // If not, use the default value defined in the header file. + // If not, use the default value defined at declaration for (const auto& [key, value] : params_) { auto type = container_node_->get_parameter(key).get_type(); From d938a69752a7e9e13801eb2bc748c08e4fd9cd55 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 26 Jun 2024 15:08:09 -0600 Subject: [PATCH 5/8] updates to path planner comments and structure --- rosplane/include/path_planner.hpp | 176 ++++++++++++++++++++++++++++ rosplane/src/path_planner.cpp | 183 +++++------------------------- 2 files changed, 207 insertions(+), 152 deletions(-) create mode 100644 rosplane/include/path_planner.hpp diff --git a/rosplane/include/path_planner.hpp b/rosplane/include/path_planner.hpp new file mode 100644 index 0000000..a44d2e3 --- /dev/null +++ b/rosplane/include/path_planner.hpp @@ -0,0 +1,176 @@ +#include "rclcpp/rclcpp.hpp" +#include "rosplane_msgs/msg/waypoint.hpp" +#include "rosplane_msgs/msg/state.hpp" +#include "rosplane_msgs/srv/add_waypoint.hpp" +#include "rosflight_msgs/srv/param_file.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +#define EARTH_RADIUS 6378145.0f + +namespace rosplane +{ + +class path_planner : public rclcpp::Node +{ +public: + path_planner(); + ~path_planner(); + + param_manager params; /** Holds the parameters for the path_planner*/ + +private: + /** + * Publishes waypoint objects + */ + rclcpp::Publisher::SharedPtr waypoint_publisher_; + + /** + * Subscribes to Vehicle state + */ + rclcpp::Subscription::SharedPtr state_subscription_; + + /** + * Service handle that publishes the next unpublished waypoint in the waypoint list + */ + rclcpp::Service::SharedPtr next_waypoint_service_; + + /** + * Service handle that clears the waypoint list and publishes a waypoint with "clear_wp_list" set true + */ + rclcpp::Service::SharedPtr clear_waypoint_service_; + + /** + * Service handle that prints the loaded waypoints to the command line + */ + rclcpp::Service::SharedPtr print_waypoint_service_; + + /** + * Service handle that adds a waypoint to the waypoint list + */ + rclcpp::Service::SharedPtr add_waypoint_service_; + + /** + * Service handle that loads a list of waypoints (i.e., a mission) from a file + */ + rclcpp::Service::SharedPtr load_mission_service_; + + /** + * @brief "publish_next_waypoint" service callback. Publish the next waypoint from the internal vector of waypoint objects. Will not publish if there are no more waypoints in the vector. + * + * @param req: Pointer to a std_srvs::srv::Trigger request object + * @param res: Pointer to a std_srvs::srv::Trigger response object + * + * @return True if waypoint was published, false if no more waypoints were available to publish + */ + bool publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res); + + /** + * @brief "add_waypoint" service callback. Adds a waypoint to the end of the vector of waypoint objects and optionally publishes another waypoint. + * + * @param req: Pointer to an AddWaypoint service request object + * @param req: Pointer to an AddWaypoint service response object + * + * @return True + */ + bool update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req, + const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res); + + /** + * @brief "clear_path" service callback. Clears all the waypoints internally and sends clear commands to path_manager + * + * @param req: Pointer to a Trigger service request object + * @param req: Pointer to a Trigger service response object + * + * @return True + */ + bool clear_path_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res); void clear_path(); + + /** + * @brief "print_path" service callback. Prints waypoints to the terminal + * + * @param req: Pointer to a Trigger service request object + * @param req: Pointer to a Trigger service response object + * + * @return True + */ + bool print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res); + + /** + * @brief "load_mission_from_file" service callback + * + * @param req: Pointer to a ParamFile service request object + * @param req: Pointer to a ParamFile service response object + * + * @return True + */ + bool load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req, + const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res); + + /** + * @brief Parses YAML file and loads waypoints + * + * @param filename: String containing the path to the YAML file + * + * @return True if loading waypoints was successful, false otherwise + */ + bool load_mission_from_file(const std::string& filename); + + /** + * @brief Callback for the rosplane_msgs::msg::State publisher. Saves the initial GNSS coordinates + * + * @param msg: Pointer to a rosplane_msgs::msg::State object + */ + void state_callback(const rosplane_msgs::msg::State & msg); + + /** + * @brief Publishes the next waypoint in the vector of waypoints + */ + void waypoint_publish(); + + /** + * @brief Publishes the number of initial waypoints given by parameter + */ + void publish_initial_waypoints(); + + /** + * @brief Converts an LLA coordinate to NED coordinates + * + * @param lla: Array of floats of size 3, with [latitude, longitude, altitude] + * @return Array of doubles corresponding to the NED coordinates measured from the origin + */ + std::array lla2ned(std::array lla); + + /** + * @brief This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. It also sets the default parameter, which will then be overridden by a launch script. + */ + void declare_parameters(); + + OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + /** + * @brief Parameter change callback + * + * @param parameters: Vector of rclcpp::Parameter that have been changed. + */ + rcl_interfaces::msg::SetParametersResult + parametersCallback(const std::vector & parameters); + + int num_waypoints_published_; + double initial_lat_; + double initial_lon_; + double initial_alt_; + + /** + * Vector of waypoints + */ + std::vector wps; +}; \ No newline at end of file diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index 8ecf304..afaeaba 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -1,8 +1,3 @@ -#include "rclcpp/rclcpp.hpp" -#include "rosplane_msgs/msg/waypoint.hpp" -#include "rosplane_msgs/msg/state.hpp" -#include "rosplane_msgs/srv/add_waypoint.hpp" -#include "rosflight_msgs/srv/param_file.hpp" #include #include #include @@ -11,143 +6,15 @@ #include #include #include - -#define EARTH_RADIUS 6378145.0f +#include "rclcpp/rclcpp.hpp" +#include "rosplane_msgs/msg/waypoint.hpp" +#include "rosplane_msgs/msg/state.hpp" +#include "rosplane_msgs/srv/add_waypoint.hpp" +#include "rosflight_msgs/srv/param_file.hpp" +#include "path_planner.hpp" using std::placeholders::_1; using std::placeholders::_2; -using namespace std::chrono_literals; - -namespace rosplane -{ - -class path_planner : public rclcpp::Node -{ -public: - path_planner(); - ~path_planner(); - - param_manager params; /** Holds the parameters for the path_planner*/ - -private: - rclcpp::Publisher::SharedPtr waypoint_publisher_; - rclcpp::Subscription::SharedPtr state_subscription_; - rclcpp::Service::SharedPtr next_waypoint_service_; - rclcpp::Service::SharedPtr clear_waypoint_service_; - rclcpp::Service::SharedPtr print_waypoint_service_; - rclcpp::Service::SharedPtr add_waypoint_service_; - rclcpp::Service::SharedPtr load_mission_service_; - - /** - * @brief "publish_next_waypoint" service callback. Publish the next waypoint from the internal vector of waypoint objects. Will not publish if there are no more waypoints in the vector. - * - * @param req: Pointer to a std_srvs::srv::Trigger request object - * @param res: Pointer to a std_srvs::srv::Trigger response object - * - * @return True if waypoint was published, false if no more waypoints were available to publish - */ - bool publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res); - - /** - * @brief "add_waypoint" service callback. Adds a waypoint to the end of the vector of waypoint objects and optionally publishes another waypoint. - * - * @param req: Pointer to an AddWaypoint service request object - * @param req: Pointer to an AddWaypoint service response object - * - * @return True - */ - bool update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req, - const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res); - - /** - * @brief "clear_path" service callback. Clears all the waypoints internally and sends clear commands to path_manager - * - * @param req: Pointer to a Trigger service request object - * @param req: Pointer to a Trigger service response object - * - * @return True - */ - bool clear_path_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res); void clear_path(); - - /** - * @brief "print_path" service callback. Prints waypoints to the terminal - * - * @param req: Pointer to a Trigger service request object - * @param req: Pointer to a Trigger service response object - * - * @return True - */ - bool print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res); - - /** - * @brief "load_mission_from_file" service callback - * - * @param req: Pointer to a ParamFile service request object - * @param req: Pointer to a ParamFile service response object - * - * @return True - */ - bool load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req, - const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res); - - /** - * @brief Parses YAML file and loads waypoints - * - * @param filename: String containing the path to the YAML file - * - * @return True if loading waypoints was successful, false otherwise - */ - bool load_mission_from_file(const std::string& filename); - - /** - * @brief Callback for the rosplane_msgs::msg::State publisher. Saves the initial GNSS coordinates - * - * @param msg: Pointer to a rosplane_msgs::msg::State object - */ - void state_callback(const rosplane_msgs::msg::State & msg); - - /** - * @brief Publishes the next waypoint in the vector of waypoints - */ - void waypoint_publish(); - - /** - * @brief Publishes the number of initial waypoints given by parameter - */ - void publish_initial_waypoints(); - - /** - * @brief Converts an LLA coordinate to NED coordinates - * - * @param lla: Array of floats of size 3, with [latitude, longitude, altitude] - * @return Array of doubles corresponding to the NED coordinates measured from the origin - */ - std::array lla2ned(std::array lla); - - /** - * @brief This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. It also sets the default parameter, which will then be overridden by a launch script. - */ - void declare_parameters(); - - OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - /** - * @brief Parameter change callback - * - * @param parameters: Vector of rclcpp::Parameter that have been changed. - */ - rcl_interfaces::msg::SetParametersResult - parametersCallback(const std::vector & parameters); - - int num_waypoints_published; - double initial_lat_; - double initial_lon_; - double initial_alt_; - - std::vector wps; -}; path_planner::path_planner() : Node("path_planner"), params(this) @@ -180,13 +47,14 @@ path_planner::path_planner() parameter_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&path_planner::parametersCallback, this, std::placeholders::_1)); + // Declare parameters with ROS2 and save them to the param_manager object declare_parameters(); params.set_parameters(); - num_waypoints_published = 0; + num_waypoints_published_ = 0; // Initialize by publishing a clear path command. - // This makes sure rviz or other vizualization tools don't show stale waypoints if rosplane is restarted. + // This makes sure rviz or other vizualization tools don't show stale waypoints if ROSplane is restarted. clear_path(); // Publishes the initial waypoints @@ -200,13 +68,15 @@ void path_planner::publish_initial_waypoints() { RCLCPP_INFO_STREAM(this->get_logger(), "Path Planner will publish the first {" << num_waypoints_to_publish_at_start << "} available waypoints!"); - while (num_waypoints_published < num_waypoints_to_publish_at_start && num_waypoints_published < (int) wps.size()) { + // Publish the first waypoints as defined by the num_waypoints_to_publish_at_start parameter + while (num_waypoints_published_ < num_waypoints_to_publish_at_start && num_waypoints_published_ < (int) wps.size()) { waypoint_publish(); } } void path_planner::state_callback(const rosplane_msgs::msg::State & msg) { - // Make sure initial LLA is not zero before updating to avoid errors + // Make sure initial LLA is not zero before updating to avoid initialization errors + // TODO: What if we want to initialize it at (0,0,0)? if (fabs(msg.initial_lat) > 0.0 || fabs(msg.initial_lon) > 0.0 || fabs(msg.initial_alt) > 0.0) { initial_lat_ = msg.initial_lat; initial_lon_ = msg.initial_lon; @@ -217,10 +87,11 @@ void path_planner::state_callback(const rosplane_msgs::msg::State & msg) { bool path_planner::publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { - if (num_waypoints_published < (int) wps.size()) { + // Publish the next waypoint, if available + if (num_waypoints_published_ < (int) wps.size()) { RCLCPP_INFO_STREAM( this->get_logger(), - "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published + 1); + "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published_ + 1); waypoint_publish(); @@ -236,12 +107,12 @@ bool path_planner::publish_next_waypoint(const std_srvs::srv::Trigger::Request:: void path_planner::waypoint_publish() { - rosplane_msgs::msg::Waypoint new_waypoint = wps[num_waypoints_published]; - new_waypoint.clear_wp_list = false; + // Publish the next waypoint off the list + rosplane_msgs::msg::Waypoint new_waypoint = wps[num_waypoints_published_]; waypoint_publisher_->publish(new_waypoint); - num_waypoints_published++; + num_waypoints_published_++; } bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req, @@ -253,11 +124,12 @@ bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::S new_waypoint.header.stamp = now; - // Convert to NED if given in LLA + // Create a string flag for the debug message (defaults to "LLA") std::string lla_or_ned = "LLA"; + + // Convert to NED if given in LLA if (req->lla) { std::array ned = lla2ned(req->w); - new_waypoint.w[0] = ned[0]; new_waypoint.w[1] = ned[1]; new_waypoint.w[2] = ned[2]; @@ -266,6 +138,8 @@ bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::S new_waypoint.w = req->w; lla_or_ned = "NED"; } + + // Fill in the Waypoint object with the information from the service request object new_waypoint.chi_d = req->chi_d; new_waypoint.lla = req->lla; new_waypoint.use_chi = req->use_chi; @@ -273,7 +147,8 @@ bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::S new_waypoint.set_current = req->set_current; if (req->publish_now) { - wps.insert(wps.begin() + num_waypoints_published, new_waypoint); + // Insert the waypoint in the correct location in the list and publish it + wps.insert(wps.begin() + num_waypoints_published_, new_waypoint); waypoint_publish(); res->message = "Adding " + lla_or_ned + " waypoint was successful! Waypoint published."; } @@ -297,12 +172,13 @@ bool path_planner::clear_path_callback(const std_srvs::srv::Trigger::Request::Sh void path_planner::clear_path() { wps.clear(); + // Publish a waypoint with "clear_wp_list" set to true to let downstream subscribers know they need to clear their waypoints rosplane_msgs::msg::Waypoint new_waypoint; new_waypoint.clear_wp_list = true; waypoint_publisher_->publish(new_waypoint); - num_waypoints_published = 0; + num_waypoints_published_ = 0; } bool path_planner::print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, @@ -390,6 +266,8 @@ std::array path_planner::lla2ned(std::array lla) { double e = EARTH_RADIUS * cos(lat0 * M_PI / 180.0) * (lon1 - lon0) * M_PI / 180.0; double d = -(alt1 - alt0); + // Usually will not be flying exactly at these locations. + // If the GPS reports (0,0,0), it most likely means there is an error with the GPS if (fabs(initial_lat_) == 0.0 || fabs(initial_lon_) == 0.0 || fabs(initial_alt_) == 0.0) { RCLCPP_WARN_STREAM(this->get_logger(), "NED position set to [" << n << "," << e << "," << d << "]! Waypoints may be incorrect. Check GPS health"); } @@ -404,6 +282,7 @@ path_planner::parametersCallback(const std::vector & paramete result.successful = false; result.reason = "One of the parameters given is not a parameter of the controller node."; + // Set parameters in the param_manager object bool success = params.set_parameters_callback(parameters); if (success) { From b7f60135764d737274eaa010e9c791bf41b96f89 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 26 Jun 2024 15:55:29 -0600 Subject: [PATCH 6/8] added some path_follower comments --- rosplane/include/path_follower_base.hpp | 44 ++++++++++++++++++++----- rosplane/src/path_follower_base.cpp | 26 +++++++-------- rosplane/src/path_follower_example.cpp | 16 ++++----- 3 files changed, 56 insertions(+), 30 deletions(-) diff --git a/rosplane/include/path_follower_base.hpp b/rosplane/include/path_follower_base.hpp index f0394b4..84f81e9 100644 --- a/rosplane/include/path_follower_base.hpp +++ b/rosplane/include/path_follower_base.hpp @@ -57,38 +57,64 @@ class path_follower_base : public rclcpp::Node param_manager params; private: + /** + * Subscribes to state from the estimator + */ rclcpp::Subscription::SharedPtr vehicle_state_sub_; + + /** + * Subscribes to the current_path topic from the path manager + */ rclcpp::Subscription::SharedPtr current_path_sub_; + /** + * Publishes commands to the controller + */ rclcpp::Publisher::SharedPtr controller_commands_pub_; - bool params_initialized_; std::chrono::microseconds timer_period_; rclcpp::TimerBase::SharedPtr update_timer_; + bool params_initialized_; + bool state_init_; + bool current_path_init_; + + OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + rosplane_msgs::msg::ControllerCommands controller_commands_; + struct input_s input_; + /** * @brief Sets the timer with the timer period as specified by the ROS2 parameters */ void set_timer(); - rosplane_msgs::msg::ControllerCommands controller_commands_; - struct input_s input_; - + /** + * @brief Callback for the subscribed state messages from the estimator + */ void vehicle_state_callback(const rosplane_msgs::msg::State::SharedPtr msg); - bool state_init_; + + /** + * @brief Callback for the subscribed current_path messages from the path_manager + */ void current_path_callback(const rosplane_msgs::msg::CurrentPath::SharedPtr msg); - bool current_path_init_; + /** + * @brief Calculates and publishes the commands messages + */ void update(); - OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - + /** + * @brief Callback for when ROS2 parameters change. + * + * @param Vector of rclcpp::Parameter objects that have changed + * @return SetParametersResult object that describes the success or failure of the request + */ rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector & parameters); /** * This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. - * It also sets the default parameter, which can be overridden by a launch script. + * It also sets the default parameter, which can be overridden by a parameter file */ void declare_parameters(); }; diff --git a/rosplane/src/path_follower_base.cpp b/rosplane/src/path_follower_base.cpp index ea36fda..f9b1b44 100644 --- a/rosplane/src/path_follower_base.cpp +++ b/rosplane/src/path_follower_base.cpp @@ -10,13 +10,15 @@ path_follower_base::path_follower_base() { vehicle_state_sub_ = this->create_subscription( "estimated_state", 10, std::bind(&path_follower_base::vehicle_state_callback, this, _1)); + current_path_sub_ = this->create_subscription( "current_path", 100, - std::bind(&path_follower_base::current_path_callback, this, _1)); // the 1 may need to be 100 + std::bind(&path_follower_base::current_path_callback, this, _1)); controller_commands_pub_ = this->create_publisher("controller_commands", 1); + // Define the callback to handle on_set_parameter_callback events parameter_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&path_follower_base::parametersCallback, this, std::placeholders::_1)); @@ -34,6 +36,7 @@ path_follower_base::path_follower_base() } void path_follower_base::set_timer() { + // Convert the frequency to a period in microseconds double frequency = params.get_double("controller_commands_pub_frequency"); timer_period_ = std::chrono::microseconds(static_cast(1.0 / frequency * 1e6)); @@ -54,22 +57,13 @@ void path_follower_base::update() rclcpp::Time now = this->get_clock()->now(); + // Populate the message with the required information msg.header.stamp = now; - msg.chi_c = output.chi_c; msg.va_c = output.va_c; msg.h_c = output.h_c; msg.phi_ff = output.phi_ff; - RCLCPP_DEBUG_STREAM(this->get_logger(), "Publishing Contoller Commands!"); - - RCLCPP_DEBUG_STREAM(this->get_logger(), "chi_c: " << msg.chi_c); - RCLCPP_DEBUG_STREAM(this->get_logger(), "va_c: " << msg.va_c); - RCLCPP_DEBUG_STREAM(this->get_logger(), "h_c: " << msg.h_c); - RCLCPP_DEBUG_STREAM(this->get_logger(), "phi_ff: " << msg.phi_ff); - - // RCLCPP_DEBUG_STREAM(this->get_logger(), "k_orbit: " << params.get_double("k_orbit")); - controller_commands_pub_->publish(msg); } } @@ -90,10 +84,14 @@ void path_follower_base::vehicle_state_callback(const rosplane_msgs::msg::State: void path_follower_base::current_path_callback(const rosplane_msgs::msg::CurrentPath::SharedPtr msg) { - if (msg->path_type == msg->LINE_PATH) input_.p_type = path_type::Line; - else if (msg->path_type == msg->ORBIT_PATH) + if (msg->path_type == msg->LINE_PATH) { + input_.p_type = path_type::Line; + } + else if (msg->path_type == msg->ORBIT_PATH) { input_.p_type = path_type::Orbit; + } + // Populate the input message with the correct information input_.va_d = msg->va_d; for (int i = 0; i < 3; i++) { input_.r_path[i] = msg->r[i]; @@ -112,6 +110,7 @@ path_follower_base::parametersCallback(const std::vector & pa result.successful = false; result.reason = "One of the parameters given is not a parameter of the path_follower node"; + // Update the param_manager object with the new parameters bool success = params.set_parameters_callback(parameters); if (success) { @@ -119,6 +118,7 @@ path_follower_base::parametersCallback(const std::vector & pa result.reason = "success"; } + // Check to see if the timer frequency parameter has changed if (params_initialized_ && success) { double frequency = params.get_double("controller_commands_pub_frequency"); diff --git a/rosplane/src/path_follower_example.cpp b/rosplane/src/path_follower_example.cpp index 7bcd487..f289c56 100644 --- a/rosplane/src/path_follower_example.cpp +++ b/rosplane/src/path_follower_example.cpp @@ -6,7 +6,6 @@ namespace rosplane double wrap_within_180(double fixed_heading, double wrapped_heading) { - // wrapped_heading - number_of_times_to_wrap * 2pi return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; } @@ -21,22 +20,20 @@ void path_follower_example::follow(const input_s & input, double chi_infty = params.get_double("chi_infty"); double gravity = params.get_double("gravity"); - if (input.p_type == path_type::Line) // follow straight line path specified by r and q - { + // If path_type is a line, follow straight line path specified by r and q + // Otherwise, follow an orbit path specified by c_orbit, rho_orbit, and lam_orbit + if (input.p_type == path_type::Line) { // compute wrapped version of the path angle float chi_q = atan2f(input.q_path[1], input.q_path[0]); chi_q = wrap_within_180(input.chi, chi_q); RCLCPP_DEBUG_STREAM(this->get_logger(), "input.chi: " << input.chi); - RCLCPP_DEBUG_STREAM(this->get_logger(), "chi_q: " << chi_q); float path_error = -sinf(chi_q) * (input.pn - input.r_path[0]) + cosf(chi_q) * (input.pe - input.r_path[1]); - // RCLCPP_INFO_STREAM(this->get_logger(), "k_path: " << k_path); - // heading command output.chi_c = chi_q - chi_infty * 2 / M_PI * atanf(k_path * path_error); @@ -44,14 +41,16 @@ void path_follower_example::follow(const input_s & input, float h_d = -input.r_path[2] - sqrtf(powf((input.r_path[0] - input.pn), 2) + powf((input.r_path[1] - input.pe), 2)) * (input.q_path[2]) / sqrtf(powf(input.q_path[0], 2) + powf(input.q_path[1], 2)); + // commanded altitude is desired altitude output.h_c = h_d; output.phi_ff = 0.0; - } else // follow an orbit path specified by c_orbit, rho_orbit, and lam_orbit - { + } + else { float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + powf((input.pe - input.c_orbit[1]), 2)); // distance from orbit center + // compute wrapped version of angular position on orbit float varphi = atan2f(input.pe - input.c_orbit[1], input.pn - input.c_orbit[0]); @@ -70,6 +69,7 @@ void path_follower_example::follow(const input_s & input, output.chi_c = wrap_within_180(0.0, output.chi_c); } + output.va_c = input.va_d; } From e144076298a9269400c5d11fcd5a40c248d92af8 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Wed, 26 Jun 2024 16:19:13 -0600 Subject: [PATCH 7/8] added some path manager documentation --- rosplane/include/path_manager_base.hpp | 32 +++++++--- rosplane/include/path_manager_example.hpp | 78 +++++++++++++++++++++-- rosplane/src/path_manager_base.cpp | 40 ++++++++---- rosplane/src/path_manager_example.cpp | 30 ++++----- 4 files changed, 138 insertions(+), 42 deletions(-) diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp index dd75048..09ebb88 100644 --- a/rosplane/include/path_manager_base.hpp +++ b/rosplane/include/path_manager_base.hpp @@ -14,14 +14,14 @@ #include #include #include -#include // src/rosplane_msgs/msg/State.msg +#include #include #include #include #include #include #include -//#include !!! + using std::placeholders::_1; using namespace std::chrono_literals; @@ -42,7 +42,7 @@ class path_manager_base : public rclcpp::Node float va_d; }; - std::vector waypoints_; + std::vector waypoints_; /** Vector of waypoints maintained by path_manager */ int num_waypoints_; int idx_a_; /** index to the waypoint that was most recently achieved */ @@ -68,8 +68,14 @@ class path_manager_base : public rclcpp::Node int8_t lamda; /** Direction of orbital path (cw is 1, ccw is -1) */ }; - param_manager params; /** Holds the parameters for the path_manager and children */ + param_manager params_; /** Holds the parameters for the path_manager and children */ + /** + * @brief Manages the current path based on the stored waypoint list + * + * @param input: input_s object that contains information about the waypoint + * @param output: output_s object that contains the parameters for the desired type of line, based on the current and next waypoints + */ virtual void manage(const struct input_s & input, struct output_s & output) = 0; @@ -84,15 +90,23 @@ class path_manager_base : public rclcpp::Node rosplane_msgs::msg::State vehicle_state_; /**< vehicle state */ bool params_initialized_; + bool state_init_; std::chrono::microseconds timer_period_; rclcpp::TimerBase::SharedPtr update_timer_; + OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - 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 vehicle_state_callback(const rosplane_msgs::msg::State & msg); /** subscribes to the estimated state from the estimator */ + void new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg); /** subscribes to waypoint messages from the path_planner */ + void current_path_publish(); /** Publishes the current path to the path follower */ - OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; + + /** + * @brief Callback that gets triggered when a ROS2 parameter is changed + * + * @param parameters: Vector of rclcpp::Parameter objects + * + * @return SetParametersResult object with the success of the parameter change + */ rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector & parameters); diff --git a/rosplane/include/path_manager_example.hpp b/rosplane/include/path_manager_example.hpp index 72a2916..8cf7df8 100644 --- a/rosplane/include/path_manager_example.hpp +++ b/rosplane/include/path_manager_example.hpp @@ -3,12 +3,13 @@ #include "path_manager_base.hpp" #include -//#include #define M_PI_F 3.14159265358979323846f #define M_PI_2_F 1.57079632679489661923f + namespace rosplane { + enum class fillet_state { STRAIGHT, @@ -33,20 +34,66 @@ class path_manager_example : public path_manager_base private: std::chrono::time_point start_time; + fillet_state fil_state_; + + /** + * @brief Determines the line type and calculates the line parameters to publish to path_follower + */ virtual void manage(const struct input_s & input, struct output_s & output); + + /** + * @brief Calculates the most convenient orbit direction based on the orientation of the vehicle relative to the orbit center + * + * @param pn: North position of the vehicle + * @param p3: East position of the vehicle + * @param c_n: North position of the orbit center + * @param c_e: East position of the orbit center + * + * @return Integer value representing the orbit direction (-1 or 1) + */ int orbit_direction(float pn, float pe, float chi, float c_n, float c_e); + + /** + * @brief Increments the indices of the waypoints currently used in the "manage" calculations + * + * @param idx_a: Index of the most recently achieved point + * @param idx_b: Index of the next target waypoint + * @param idx_c: Index of the waypoint after the next target waypoint + * @param input: Struct containing the state of the vehicle + * @param output: Struct that will contain all of the information about the desired line to pass to the path follower + */ void increment_indices(int & idx_a, int & idx_b, int & idx_c, const struct input_s & input, struct output_s & output); + /** + * @brief Manages a straight line segment. Calculates the appropriate line parameters to send to the path follower + * + * @param input: Input struct that contains some of the state of the vehicle + * @param output: Output struct containing the information about the desired line + */ void manage_line(const struct input_s & input, struct output_s & output); + + /** + * @brief Manages a fillet line segment. Calculates the appropriate line parameters to send to the path follower + * + * @param input: Input struct that contains some of the state of the vehicle + * @param output: Output struct containing the information about the desired line + */ void manage_fillet(const struct input_s & input, struct output_s & output); - fillet_state fil_state_; - void construct_straight_path(); + + /** + * @brief Manages a Dubins path segment. Calculates the appropriate line parameters to send to the path follower + * + * @param input: Input struct that contains some of the state of the vehicle + * @param output: Output struct containing the information about the desired line + */ void manage_dubins(const struct input_s & input, struct output_s & output); + dubin_state dub_state_; + struct dubinspath_s { @@ -67,15 +114,38 @@ class path_manager_example : public path_manager_base Eigen::Vector3f q3; /** unit vector defining direction of half plane H3 */ }; struct dubinspath_s dubinspath_; + + /** + * @brief Calculates the parameters of a Dubins path + * + * @param start_node: Starting waypoint of the Dubins path + * @param end_node: Ending waypoint of the Dubins path + * @param R: Minimum turning radius R + */ void dubinsParameters(const struct waypoint_s start_node, const struct waypoint_s end_node, float R); + /** + * @brief Computes the rotation matrix for a rotation in the z plane (normal to the Dubins plane) + * + * @param theta: Rotation angle + * + * @return 3x3 rotation matrix + */ Eigen::Matrix3f rotz(float theta); + + /** + * @brief Wraps an angle to 2*PI + * + * @param in: Angle to wrap + * + * @return Angle wrapped to within 2*PI + */ float mo(float in); /** * This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. - * It also sets the default parameter, which will then be overridden by a launch script. + * It also sets the default parameter, which will then be overridden by a parameter file */ void declare_parameters(); }; diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp index 8d7984e..9f2b1bb 100644 --- a/rosplane/src/path_manager_base.cpp +++ b/rosplane/src/path_manager_base.cpp @@ -10,7 +10,7 @@ namespace rosplane { path_manager_base::path_manager_base() - : Node("rosplane_path_manager"), params(this), params_initialized_(false) + : Node("rosplane_path_manager"), params_(this), params_initialized_(false) { vehicle_state_sub_ = this->create_subscription( "estimated_state", 10, std::bind(&path_manager_base::vehicle_state_callback, this, _1)); @@ -22,8 +22,9 @@ path_manager_base::path_manager_base() parameter_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&path_manager_base::parametersCallback, this, std::placeholders::_1)); + // Declare parameters maintained by this node with ROS2. Required for all ROS2 parameters associated with this node declare_parameters(); - params.set_parameters(); + params_.set_parameters(); params_initialized_ = true; @@ -36,13 +37,15 @@ 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("R_min", 50.0); + params_.declare_double("current_path_pub_frequency", 100.0); } void path_manager_base::set_timer() { - double frequency = params.get_double("current_path_pub_frequency"); + // Calculate the period in milliseconds from the frequency + double frequency = params_.get_double("current_path_pub_frequency"); timer_period_ = std::chrono::microseconds(static_cast(1.0 / frequency * 1e6)); + update_timer_ = this->create_wall_timer(timer_period_, std::bind(&path_manager_base::current_path_publish, this)); } @@ -54,15 +57,17 @@ path_manager_base::parametersCallback(const std::vector & par result.successful = false; result.reason = "One of the parameters given is not a parameter of the controller node."; - bool success = params.set_parameters_callback(parameters); + // Update the changed parameters in the param_manager object + bool success = params_.set_parameters_callback(parameters); if (success) { result.successful = true; result.reason = "success"; } + // If the frequency parameter was changed, restart the timer. if (params_initialized_ && success) { - double frequency = params.get_double("current_path_pub_frequency"); + double frequency = params_.get_double("current_path_pub_frequency"); std::chrono::microseconds curr_period = std::chrono::microseconds(static_cast(1.0 / frequency * 1e6)); if (timer_period_ != curr_period) { update_timer_->cancel(); @@ -83,9 +88,10 @@ 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 R_min = params_.get_double("R_min"); orbit_dir = 0; + // If the message contains "clear_wp_list", then clear all waypoints and do nothing else if (msg.clear_wp_list == true) { waypoints_.clear(); num_waypoints_ = 0; @@ -93,6 +99,8 @@ void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint return; } + // If there are currently no waypoints in the list, then add a temporary waypoint as + // the current state of the aircraft. This is necessary to define a line for line following. if (waypoints_.size() == 0) { waypoint_s temp_waypoint; @@ -121,6 +129,7 @@ void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint nextwp.chi_d = msg.chi_d; nextwp.use_chi = msg.use_chi; nextwp.va_d = msg.va_d; + // Save the last waypoint for comparison. if (waypoints_.size() > 0) { @@ -139,7 +148,7 @@ void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint } } -void path_manager_base::current_path_publish() //const rclcpp::TimerEvent & +void path_manager_base::current_path_publish() { struct input_s input; @@ -150,17 +159,22 @@ void path_manager_base::current_path_publish() //const rclcpp::TimerEvent & struct output_s output; - if (state_init_ == true) { manage(input, output); } + if (state_init_ == true) { + manage(input, output); + } rosplane_msgs::msg::CurrentPath current_path; rclcpp::Time now = this->get_clock()->now(); + // Populate current_path message current_path.header.stamp = now; - - if (output.flag) current_path.path_type = current_path.LINE_PATH; - else + if (output.flag) { + current_path.path_type = current_path.LINE_PATH; + } + else { current_path.path_type = current_path.ORBIT_PATH; + } current_path.va_d = output.va_d; for (int i = 0; i < 3; i++) { current_path.r[i] = output.r[i]; diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp index d74978d..920dfbe 100644 --- a/rosplane/src/path_manager_example.cpp +++ b/rosplane/src/path_manager_example.cpp @@ -19,18 +19,17 @@ path_manager_example::path_manager_example() // Declare the parameters used in this class with the ROS2 system declare_parameters(); - params.set_parameters(); + params_.set_parameters(); start_time = std::chrono::system_clock::now(); - } void path_manager_example::manage(const input_s & input, output_s & output) { // For readability, declare the parameters that will be used in the function here - double R_min = params.get_double("R_min"); - double default_altitude = params.get_double("default_altitude"); // This is the true altitude not the down position (no need for a negative) - double default_airspeed = params.get_double("default_airspeed"); + double R_min = params_.get_double("R_min"); + double default_altitude = params_.get_double("default_altitude"); // This is the true altitude not the down position (no need for a negative) + double default_airspeed = params_.get_double("default_airspeed"); if (num_waypoints_ == 0) { @@ -74,7 +73,7 @@ void path_manager_example::manage_line(const input_s & input, output_s & output) { // For readability, declare the parameters that will be used in the function here - bool orbit_last = params.get_bool("orbit_last"); + bool orbit_last = params_.get_bool("orbit_last"); Eigen::Vector3f p; p << input.pn, input.pe, -input.h; @@ -112,7 +111,7 @@ void path_manager_example::manage_line(const input_s & input, n_i = q_im1; } - // If the aircraft passes through the plane that bisects the angle between the waypoint lines transition. + // If the aircraft passes through the plane that bisects the angle between the waypoint lines, transition. if ((p - w_i).dot(n_i) > 0.0f) { if (idx_a_ == num_waypoints_ - 1) { idx_a_ = 0; @@ -126,8 +125,8 @@ void path_manager_example::manage_fillet(const input_s & input, output_s & output) { // For readability, declare the parameters that will be used in the function here - bool orbit_last = params.get_bool("orbit_last"); - double R_min = params.get_double("R_min"); + bool orbit_last = params_.get_bool("orbit_last"); + double R_min = params_.get_double("R_min"); if (num_waypoints_ < 3) // Do not attempt to fillet between only 2 points. { @@ -268,7 +267,7 @@ void path_manager_example::manage_dubins(const input_s & input, output_s & output) { // For readability, declare the parameters that will be used in the function here - double R_min = params.get_double("R_min"); + double R_min = params_.get_double("R_min"); Eigen::Vector3f p; p << input.pn, input.pe, -input.h; @@ -421,7 +420,6 @@ void path_manager_example::dubinsParameters(const waypoint_s start_node, const w float ell = sqrtf((start_node.w[0] - end_node.w[0]) * (start_node.w[0] - end_node.w[0]) + (start_node.w[1] - end_node.w[1]) * (start_node.w[1] - end_node.w[1])); if (ell < 2.0 * R) { - //ROS_ERROR("The distance between nodes must be larger than 2R."); RCLCPP_ERROR(this->get_logger(), "The distance between nodes must be larger than 2R."); } else { @@ -564,9 +562,9 @@ 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); + 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) @@ -600,8 +598,8 @@ int path_manager_example::orbit_direction(float pn, float pe, float chi, float c void path_manager_example::increment_indices(int & idx_a, int & idx_b, int & idx_c, const struct input_s & input, struct output_s & output) { - bool orbit_last = params.get_bool("orbit_last"); - double R_min = params.get_double("R_min"); + bool orbit_last = params_.get_bool("orbit_last"); + double R_min = params_.get_double("R_min"); if (temp_waypoint_ && idx_a_ == 1) { From ca746e726b84c5ad4dc0e180aaaa838ad09b5187 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Thu, 27 Jun 2024 12:08:19 -0600 Subject: [PATCH 8/8] removed timer from path planner --- rosplane/src/path_planner.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index afaeaba..274593d 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -66,7 +66,7 @@ path_planner::~path_planner() {} void path_planner::publish_initial_waypoints() { int num_waypoints_to_publish_at_start = this->get_parameter("num_waypoints_to_publish_at_start").as_int(); - RCLCPP_INFO_STREAM(this->get_logger(), "Path Planner will publish the first {" << num_waypoints_to_publish_at_start << "} available waypoints!"); + RCLCPP_INFO_STREAM_ONCE(this->get_logger(), "Path Planner will publish the first {" << num_waypoints_to_publish_at_start << "} available waypoints!"); // Publish the first waypoints as defined by the num_waypoints_to_publish_at_start parameter while (num_waypoints_published_ < num_waypoints_to_publish_at_start && num_waypoints_published_ < (int) wps.size()) { @@ -157,6 +157,8 @@ bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::S res->message = "Adding " + lla_or_ned + " waypoint was successful!"; } + publish_initial_waypoints(); + res->success = true; return true; } @@ -214,6 +216,7 @@ bool path_planner::load_mission(const rosflight_msgs::srv::ParamFile::Request::S const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res) { clear_path(); res->success = load_mission_from_file(req->filename); + publish_initial_waypoints(); return true; }