diff --git a/README.md b/README.md index e2bb380..5c17580 100644 --- a/README.md +++ b/README.md @@ -46,4 +46,41 @@ To run ROSplane on hardware: 2. Fly the aircraft as you would under regular RC control. 3. When ready for autonomous flight flip throttle/attitude override switch (default channel 5). 4. Fly autonomously! - - Note: the default autonomous mission is to fly a triangular loop, see path_planner_example.cpp for details. + +## Flying missions + +Autonomous waypoint missions can easily be flown using ROSplane. +The waypoints of a mission are controlled by the `path_planner` node. +These waypoints are sent to the `path_manager` node. +Low level path-following is done by the `path_follower` node. +See "Small Unmanned Aircraft: Theory and Practice" by Dr. Randy Beard and Dr. Tim McLain for more information on the architecture. + +### Adding waypoints + +ROSplane initializes with no waypoints added to the `path_planner`. +We recommend using a mission .yaml file (an example mission can be found in `rosplane/params/fixedwing_mission.yaml`). +Loading the mission can be done using + +```ros2 service call /load_mission_from_file rosflight_msgs/srv/ParamFile "{filename: FILENAME}"``` + +where `FILENAME` is the absolute path to the mission .yaml file. +Note that the origin (0,0,0) is placed at the GNSS location where ROSplane was initialized. + +> **Important**: All waypoints must include a valid `[X, Y, Z]` value and a valid `va_d` value. + +Alternatively, you can add a waypoint one at a time by calling the appropriate service, +```ros2 service call /add_waypoint rosplane_msgs/srv/AddWaypoint "{w: [X, Y, Z], chi_d: CHI_D, use_chi: USE_CHI, va_d: VA_D}"``` +where `[X, Y, Z]` is the NED position of the waypoint from the origin (in meters), `CHI_D` is the desired heading at the waypoint, and `VA_D` is the airspeed at the waypoint. +Corners in the path are controlled by `USE_CHI`, where a value of `True` will cause ROSplane to use a Dubins path planner and a value of `False` will cause a fillet path planner to be used. +Adding waypoints can be done after loading from a file. + +Clearing waypoints can be done using +```ros2 service call /clear_waypoints std_msgs/srv/Trigger```. + +### Publishing Waypoints + +The `path_planner` node automatically publishes a small number of waypoints (default is 3) at the beginning of this mission. +This number is controlled by the `num_waypoints_to_publish_at_start` ROS2 parameter. + +Additional waypoints can be published using +`ros2 service call /publish_next_waypoint std_srvs/srv/Trigger`. \ No newline at end of file diff --git a/rosplane/CMakeLists.txt b/rosplane/CMakeLists.txt index 3958b07..4eca041 100644 --- a/rosplane/CMakeLists.txt +++ b/rosplane/CMakeLists.txt @@ -31,6 +31,7 @@ find_package(geometry_msgs REQUIRED) find_package(rosplane_msgs REQUIRED) find_package(Eigen3 3.3 REQUIRED NO_MODULE) find_package(rosflight_msgs REQUIRED) +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) ament_export_dependencies(rclcpp rclpy) ament_export_include_directories(include) @@ -40,6 +41,7 @@ include_directories( #use this if you need .h files for include statements. Th include ${EIGEN3_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} + ${YAML_CPP_INCLUDEDIR} ) install(DIRECTORY launch params DESTINATION share/${PROJECT_NAME}/) @@ -83,7 +85,10 @@ install(TARGETS add_executable(rosplane_path_planner src/path_planner.cpp src/param_manager.cpp) -ament_target_dependencies(rosplane_path_planner rosplane_msgs std_srvs rclcpp rclpy Eigen3) +target_link_libraries(rosplane_path_planner + ${YAML_CPP_LIBRARIES} +) +ament_target_dependencies(rosplane_path_planner rosplane_msgs rosflight_msgs std_srvs rclcpp rclpy Eigen3) install(TARGETS rosplane_path_planner DESTINATION lib/${PROJECT_NAME}) diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp index e8d4326..ba15170 100644 --- a/rosplane/include/path_manager_base.hpp +++ b/rosplane/include/path_manager_base.hpp @@ -37,7 +37,7 @@ class path_manager_base : public rclcpp::Node { float w[3]; float chi_d; - bool chi_valid; + bool use_chi; float va_d; }; diff --git a/rosplane/params/fixedwing_mission.yaml b/rosplane/params/fixedwing_mission.yaml new file mode 100644 index 0000000..1cab008 --- /dev/null +++ b/rosplane/params/fixedwing_mission.yaml @@ -0,0 +1,21 @@ +# WAYPOINT 1 +wp: + w: [0.0, 0.0, -50.0] + chi_d: 1.1518 + use_chi: True + va_d: 25.0 +wp: + w: [200.0, 100.0, -50.0] + chi_d: 1.1518 + use_chi: True + va_d: 25.0 +wp: + w: [200.0, -100.0, -50.0] + chi_d: 1.1518 + use_chi: True + va_d: 25.0 +wp: + w: [0.0, -100.0, -50.0] + chi_d: 1.1518 + use_chi: True + va_d: 25.0 \ No newline at end of file diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp index 5fce1f6..72e3997 100644 --- a/rosplane/src/path_manager_base.cpp +++ b/rosplane/src/path_manager_base.cpp @@ -68,7 +68,7 @@ void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint currentwp.w[1] = vehicle_state_.position[1]; currentwp.w[2] = (vehicle_state_.position[2] > -25 ? msg.w[2] : vehicle_state_.position[2]); currentwp.chi_d = vehicle_state_.chi; - currentwp.chi_valid = msg.chi_valid; + currentwp.use_chi = msg.use_chi; currentwp.va_d = msg.va_d; @@ -82,7 +82,7 @@ void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint nextwp.w[1] = msg.w[1]; nextwp.w[2] = msg.w[2]; nextwp.chi_d = msg.chi_d; - nextwp.chi_valid = msg.chi_valid; + nextwp.use_chi = msg.use_chi; nextwp.va_d = msg.va_d; waypoints_.push_back(nextwp); num_waypoints_++; diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp index 57a60df..1a1ca27 100644 --- a/rosplane/src/path_manager_example.cpp +++ b/rosplane/src/path_manager_example.cpp @@ -36,7 +36,7 @@ void path_manager_example::manage(const input_s & input, output_s & output) output.rho = R_min; output.lamda = 1; } else { - if (waypoints_[idx_a_].chi_valid) { + if (waypoints_[idx_a_].use_chi) { manage_dubins(input, output); } else { /** Switch the following for flying directly to waypoints, or filleting corners */ diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index 42dba47..4ea0aad 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -1,16 +1,20 @@ #include "rclcpp/rclcpp.hpp" #include "rosplane_msgs/msg/waypoint.hpp" +#include "rosplane_msgs/srv/add_waypoint.hpp" +#include "rosflight_msgs/srv/param_file.hpp" #include #include #include #include -#include +// #include #include +#include +#include -#define num_waypoints 7 -#define num_waypoints_to_publish 3 +#define NUM_WAYPOINTS_TO_PUBLISH_AT_START 3 using std::placeholders::_1; +using std::placeholders::_2; using namespace std::chrono_literals; namespace rosplane @@ -22,115 +26,246 @@ class path_planner : public rclcpp::Node path_planner(); ~path_planner(); + param_manager params; /** Holds the parameters for the path_planner*/ + private: rclcpp::Publisher::SharedPtr waypoint_publisher; - rclcpp::Service::SharedPtr waypoint_service; + 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::TimerBase::SharedPtr timer_; bool publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res); + + bool update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req, + const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res); + + bool clear_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res); + + bool print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res); + + bool load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req, + const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res); + + bool load_mission_from_file(const std::string& filename); void waypoint_publish(); + void timer_callback(); + /** + * 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_; + rcl_interfaces::msg::SetParametersResult + parametersCallback(const std::vector & parameters); int num_waypoints_published; - float wps[5 * num_waypoints]; + std::vector wps; }; path_planner::path_planner() - : Node("path_planner") + : Node("path_planner"), params(this) { waypoint_publisher = this->create_publisher("waypoint_path", 10); - waypoint_service = this->create_service( + next_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)); + std::bind(&path_planner::publish_next_waypoint, this, _1, _2)); + + add_waypoint_service = this->create_service( + "add_waypoint", + std::bind(&path_planner::update_path, this, _1, _2)); + + clear_waypoint_service = this->create_service( + "clear_waypoints", + std::bind(&path_planner::clear_path, this, _1, _2)); + + 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_from_file", + std::bind(&path_planner::load_mission, this, _1, _2)); + + // 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)); num_waypoints_published = 0; - - float va = 25; - float wps_to_add[5 * num_waypoints] = { - 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}; - - for (int i = 0; i < 5 * num_waypoints; i++) { wps[i] = wps_to_add[i]; } } path_planner::~path_planner() {} +void path_planner::timer_callback() { + if (num_waypoints_published < NUM_WAYPOINTS_TO_PUBLISH_AT_START && num_waypoints_published < (int) wps.size()) { + waypoint_publish(); + } +} + bool path_planner::publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, const std_srvs::srv::Trigger::Response::SharedPtr & res) { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published); + if (num_waypoints_published < (int) wps.size()) { + 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; + waypoint_publish(); - rclcpp::Time now = this->get_clock()->now(); + res->success = true; + return true; + } + else { + RCLCPP_ERROR_STREAM(this->get_logger(), "No waypoints left to publish! Add more waypoints"); + res->success = false; + return false; + } +} + +void path_planner::waypoint_publish() +{ - new_waypoint.header.stamp = now; + rosplane_msgs::msg::Waypoint new_waypoint = wps[num_waypoints_published]; + new_waypoint.clear_wp_list = false; - int i = num_waypoints_published; + waypoint_publisher->publish(new_waypoint); - 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]; + num_waypoints_published++; +} + +bool path_planner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req, + const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res) { + + rosplane_msgs::msg::Waypoint new_waypoint; - 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; + rclcpp::Time now = this->get_clock()->now(); - waypoint_publisher->publish(new_waypoint); + new_waypoint.header.stamp = now; + new_waypoint.w = req->w; + new_waypoint.chi_d = req->chi_d; + new_waypoint.use_chi = req->use_chi; + new_waypoint.va_d = req->va_d; + new_waypoint.set_current = req->set_current; - num_waypoints_published++; + if (req->publish_now) { + wps.insert(wps.begin() + num_waypoints_published, new_waypoint); + waypoint_publish(); + res->message = "Adding waypoint was successful! Waypoint published."; + } + else { + wps.push_back(new_waypoint); + res->message = "Adding waypoint was successful!"; } + res->success = true; + return true; } -void path_planner::waypoint_publish() -{ +bool path_planner::clear_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) { + wps.clear(); - if (num_waypoints_published < num_waypoints_to_publish) { + rosplane_msgs::msg::Waypoint new_waypoint; + new_waypoint.clear_wp_list = true; - rosplane_msgs::msg::Waypoint new_waypoint; + waypoint_publisher->publish(new_waypoint); - rclcpp::Time now = this->get_clock()->now(); + res->success = true; + return true; +} + +bool path_planner::print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) { + std::stringstream output; + + output << "Printing waypoints..."; + + for (int i=0; i < (int) wps.size(); ++i) { + rosplane_msgs::msg::Waypoint wp = wps[i]; + output << std::endl << "----- WAYPOINT " << i << " -----" << std::endl; + output << "Position (NED, meters): [" << wp.w[0] << ", " << wp.w[1] << ", " << wp.w[2] << "]" << std::endl; + output << "Chi_d: " << wp.chi_d << " " << std::endl; + output << "Va_d: " << wp.va_d << " " << std::endl; + output << "use_chi: " << wp.use_chi; + } + + // Print to info log stream + RCLCPP_INFO_STREAM(this->get_logger(), output.str()); + + res->success = true; + + return true; +} + +bool path_planner::load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req, + const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res) { + // std::string filename = req->filename; + res->success = load_mission_from_file(req->filename); + return true; +} - new_waypoint.header.stamp = now; +bool path_planner::load_mission_from_file(const std::string& filename) { + try { + YAML::Node root = YAML::LoadFile(filename); + assert(root.IsSequence()); + RCLCPP_INFO_STREAM(this->get_logger(), root); - int i = num_waypoints_published; + for (YAML::const_iterator it=root.begin(); it!=root.end(); ++it) { + YAML::Node wp = it->second; - 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]; + rosplane_msgs::msg::Waypoint new_wp; + new_wp.w = wp["w"].as>(); + new_wp.chi_d = wp["chi_d"].as(); + new_wp.use_chi = wp["use_chi"].as(); + new_wp.va_d = wp["va_d"].as(); - 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; + wps.push_back(new_wp); + } - waypoint_publisher->publish(new_waypoint); + return true; + } + catch (...) { + return false; + } +} - num_waypoints_published++; +rcl_interfaces::msg::SetParametersResult +path_planner::parametersCallback(const std::vector & parameters) +{ + 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."; + + bool success = params.set_parameters_callback(parameters); + if (success) + { + result.successful = true; + result.reason = "success"; } + + return result; } + +void path_planner::declare_parameters() { + params.declare_int("num_waypoints_to_publish_at_start", 3); +} + } // namespace rosplane int main(int argc, char ** argv) diff --git a/rosplane_msgs/CMakeLists.txt b/rosplane_msgs/CMakeLists.txt index 50571b9..91096e5 100644 --- a/rosplane_msgs/CMakeLists.txt +++ b/rosplane_msgs/CMakeLists.txt @@ -24,13 +24,22 @@ find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) -rosidl_generate_interfaces(${PROJECT_NAME} +set(msg_files "msg/Command.msg" "msg/ControllerCommands.msg" "msg/ControllerInternalsDebug.msg" "msg/CurrentPath.msg" "msg/State.msg" "msg/Waypoint.msg" +) + +set(srv_files + "srv/AddWaypoint.srv" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} DEPENDENCIES std_msgs # Add packages that above messages depend on sensor_msgs diff --git a/rosplane_msgs/msg/Waypoint.msg b/rosplane_msgs/msg/Waypoint.msg index 74e7c7e..454374e 100644 --- a/rosplane_msgs/msg/Waypoint.msg +++ b/rosplane_msgs/msg/Waypoint.msg @@ -6,8 +6,8 @@ std_msgs/Header header # @warning w and Va_d always have to be valid; the chi_d is optional. float32[3] w # Waypoint in local NED (m) float32 chi_d # Desired course at this waypoint (rad) -bool chi_valid # Desired course valid (dubin or fillet paths) +bool use_chi # Use chi_d as specified, using a Dubin's path. Otherwise, use fillet float32 va_d # Desired airspeed (m/s) -bool set_current # Sets this waypoint to be executed now! Starts a new list +bool set_current # Sets a waypoint at the vehicle's current position bool clear_wp_list # Removes all waypoints and returns to origin. The rest of # this message will be ignored diff --git a/rosplane_msgs/srv/AddWaypoint.srv b/rosplane_msgs/srv/AddWaypoint.srv new file mode 100644 index 0000000..1229258 --- /dev/null +++ b/rosplane_msgs/srv/AddWaypoint.srv @@ -0,0 +1,12 @@ +# Service to dynamically add new waypoint + +# @warning w and Va_d always have to be valid; the chi_d is optional. +float32[3] w # Waypoint in local NED (m) +float32 chi_d # Desired course at this waypoint (rad) +bool use_chi # Desired course valid (dubin or fillet paths) +float32 va_d # Desired airspeed (m/s) +bool set_current # Sets this waypoint to be executed now! Starts a new list +bool publish_now # Immediately publishes the waypoint after adding +--- +bool success +string message \ No newline at end of file