7 dynamic waypoint publishing

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`.

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})

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;
   };
 

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

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_++;

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 */