initial path manager documentation

docs/developer-guide/rosplane/path-planning/path-planning-overview.md → docs/developer-guide/rosplane/navigation/navigation-overview.md

@@ -1,4 +1,4 @@
-# Path Planning Overview
+# Navigation Overview
 
 The path planning methods used in ROSplane is modeled after the architecture presented in *Small Unmanned Aircraft: Theory and Practice* by Dr. Randal Beard and Dr. Tim McLain.
 
@@ -23,14 +23,22 @@ The job of the path follower is to control the course, airspeed, and altitude of
 For more information on each of these nodes, see the related documentation pages for the [path planner](./path-planner.md), the [path manager](./path-manager.md), and the [path follower](./path-follower.md).
 See also chapters 10-13 of the *Small Unmanned Aircraft: Theory and Practice* book.
 
-## Changing Path Planning
+## Changing the Navigation Stack
 The path planner currently takes in a set of user-defined waypoints and follows those waypoints. 
 This is not useful in all contexts, since in many applications the aircraft needs to take in sensor data and create its own waypoints and paths.
 
-The modularity of this framework allows users to "plug in" their own algorithms for each of the blocks defined in Figure 1, instead of rewriting the whole path planning stack.
-
 For example, we could replace the path planner block with a vision-based guidance block.
 The vision-based-guidance block would make decisions based on visual data to create waypoints. 
 However, the interface between the path planner (now the visual-based guidance planner) and the path manager would remain the same.
 
-Chapter 13 of the *Small Unmanned Aircraft: Theory and Practice* book contains more detail on implementing a vision-based path planner.
+Similarly, the path manager could be adjusted to follow b-splines instead of straight lines and orbits as currently implemented, without affecting the general waypoint management (path planner) or the underlying path following control loops (path follower).
+
+The modularity of this framework allows users to "plug in" their own algorithms for each of the blocks defined in Figure 1, instead of rewriting the whole path planning stack.
+
+Chapter 13 of the *Small Unmanned Aircraft: Theory and Practice* book contains more detail on implementing a vision-based path planner.
+
+## Running the Navigation Stack
+To launch the navigation stack, you will need to launch 3 ROS2 nodes, one for each part of the navigation stack (path planner, manager, and follower).
+
+
+Remember to launch these nodes with the appropriate parameter launch file, as seen in the `rosplane.launch.py` file.

docs/developer-guide/rosplane/navigation/path-follower.md

@@ -0,0 +1,5 @@
+# Path Follower
+
+## Overview
+The path follower subscribes to the current path published by the path manager. 
+It takes in the 

docs/developer-guide/rosplane/navigation/path-manager.md

@@ -0,0 +1,77 @@
+# Path Manager
+
+## Overview
+The path manager is responsible for calculating and publishing the current path to the path follower. 
+It is split into a base class and an inherited class.
+The base class handles the ROS2 interfaces (publishing and subscribing) while the inherited class manages the path, i.e., calculates the parameters for the current path.
+
+The path manager is designed this way so that the inherited class (how the paths are managed) can be replaced or rewritten without affecting the overlying ROS2 interface.
+
+Both the `path_manager_base` and the `path_manager_example` are included in the same ROS2 node when compiled, called `path_manager`.
+Parameters associated with the `path_manager_base` or the `path_manager_example` will therefore be visible under the `path_manager` ROS2 node.
+
+More details about the path manager can be found in *Small Unmanned Aircraft: Theory and Practice* by Dr. Randal Beard and Dr. Tim McLain.
+
+## Path Manager Base
+The path manager base contains all the ROS2 interfaces required for the path manager.
+A list of these interfaces is below.
+
+### List of ROS2 Interfaces
+
+| **ROS2 Interface** | **Topic or Service** | **Explanation** | **Message or Service Type** |
+| :---: | :---: | :---: | :---: |
+| `vehicle_state_sub_` | `/estimated_state` | Subscribes to the estimated aircraft state | `rosplane_msgs::msg::State` |
+| `new_waypoint_sub_` | `/waypoint_path` | Subscribes to the waypoint messages published by `path_planner` | `rosplane_msgs::msg::State` |
+| `current_path_pub_` | `/current_path` | Publishes the parameters of the current path | `rosplane_msgs::msg::CurrentPath` |
+| `update_timer_` | -- | ROS2 timer that controls how frequently `current_path_pub_` publishes | -- |
+
+### Interface with the Path Follower
+The `path_manager` node interfaces with the path follower by publishing to the `/current_path` topic.
+These messages contain information about the current trajectory, which are either straight lines or circles. 
+
+Note that the only supported path types are straight lines and circles.
+Depending on how the `path_manager` is set up to manage the waypoints, this may not be suitable for your application.
+See "Modifying the Path Manager" for more information.
+For more information on the path follower, see the [Path Follwer](./path-follower.md) documentation.
+
+## Path Manager Example
+The `path_manager_example` class inherits from the `path_manager_base` class.
+Specifically, the `path_manager_example` overrides the `path_manager_base::manage` method to determine how the path is managed.
+
+In the current implementation, `path_manager_example` decides to manage a waypoint using straight lines and fillets, orbits, or Dubins paths based on the current state of the aircraft, the values of the current ROS2 parameters, and the waypoints given.
+For example, if the `use_chi` field on a Waypoint object (see [Path Planner](./path-planner.md) for more information) is set to `true`, then `path_manager_example` will use a Dubins path to navigate to the next waypoint.
+If `use_chi` is set to `false`, then `path_manager_example` will use straight lines when navigating in between waypoints and a fillet to manage the corners.
+See Figure 1 for an example of a real flight path flown using ROSplane with `chi_d` set to `false`.
+
+
+| ![Example of flight path with straight lines and fillets](../../../assets/path_planner_assets/path-planning-overview.svg "Example of flight path with straight lines and fillets") |
+| :--: |
+|*Example flight path from an actual flight test showing the straight lines and fillets path type*|
+
+Note that using fillets to manage the corners often means that the aircraft does not actually achieve the waypoint.
+If this is undesireable in your application, use Dubins paths or another method for the `path_manager`. 
+
+
+## Parameters
+See the [Parameter Management](../parameter-management.md) page for more details on how parameter management works.
+
+### List of Parameters
+| **Parameter** | **Explanation** | **Type** | **Range** |
+| :---: | :---: | :---: | :---: |
+| `R_min` | Minimum radius for orbit, fillet, and Dubins paths | double | $\geq$ 0.0 |
+| `orbit_last` | Specifies whether or not to orbit the last waypoint. If false, `param_manager` will fly a circuit | bool | `true` or `false` |
+
+### The `orbit_last` Parameter
+The `path_manager` node has a parameter named `orbit_last`.
+This parameter controls what happens as the aircraft is approaching the last waypoint in the set of waypoints published to the `path_manager`.
+If `orbit_last` is set to `true`, then `path_manager` will follow an orbit around the last waypoint.
+
+The direction of this waypoint (clockwise or counterclockwise) will be chosen based on which direction requires the least amount of turning.
+In other words, if the aircraft is angled slightly left as it approaches the waypoint, `path_manager` will choose to orbit the last waypoint counterclockwise (from a top-down perspective).
+If the aircraft is pointing slightly right as it approaches the waypoint, it will orbit the waypoint clockwise (from a top-down perspective).
+
+## Modifying the Path Manager
+Changes or additions to any ROS2 interfaces should be done in the `path_manager_base` field.
+
+Changes to how `path_manager` "manages" the waypoints should be done by overriding the `path_manager_base::manage` method.
+If you wish to change the way paths are defined, make sure that the `/current_path` topic is rewritten to contain the required information.

docs/developer-guide/rosplane/path-planning/path-planner.md → docs/developer-guide/rosplane/navigation/path-planner.md

@@ -1,11 +1,11 @@
-# Path planner
+# Path Planner
 
 ## Overview
 The path planner is responsible for creating, managing, and publishing waypoints.
 In the current implementation, the path planner simply maintains a list of user-defined waypoints that the user can add to or clear.
 The path planner then controls when waypoints are published to the path manager.
 
-Another implementation of the path planner (as described in [the overview](./path-planning-overview.md)) could include a visual-based path planner.
+Another implementation of the path planner (as described in [the overview](./navigation-overview.md)) could include a visual-based path planner.
 In this case, the path planner would receive sensor input and create and manage its own waypoints instead of a user-defined list of waypoints.
 However, this new path planner would still be responsible for creating and publishing waypoints.
 This ensures that the interface between the path planner and the path manager would stay the same, allowing for modularity in the path planner architecture.
@@ -17,8 +17,43 @@ This node subscribes to the topics it needs, provides services available to the
 The interface between `path_planner` and `path_manager` is the `/waypoint_path` topic,  which publishes messages with the `rosplane_msgs::msg::Waypoint` type.
 This message contains information about the waypoint's location in NED (from the origin) or GNSS (LLA) coordinates, the desired airspeed at the waypoint, and the desired heading of the aircraft at the waypoint.
 
+### About Waypoints
+The following table contains the data members of the `rosplane_msgs::msg::Waypoint` objects and a brief description.
+
+| **Member** | **Description** | **Type** | **Required** |
+| :---: | :---: | :---: | :---: |
+| `header` | Standard message header that contains a valid timestamp | `std_msgs::msg::Header` | Yes |
+| `w` | Waypoint coordinates in NED or GNSS (LLA) | `std::array<float32, 3>` | Yes |
+| `lla` | Flag to determine if the coordinates are passed in as NED or GNSS | `bool` | Yes |
+| `chi_d` | Desired course at the waypoint | `double` | No |
+| `use_chi` | Flag to use the `chi_d` value at the waypoint or not | `bool` | No - defaults to `false` |
+| `va_d` | Desired airspeed at the waypoint | `double` | Yes |
+| `clear_wp_list` | Clears all waypoints from `path_planner` and `path_manager` | `bool` | No |
+
+#### Notes on the Waypoint object fields
+The `w` field contains the coordinates of the waypoint in either NED coordinates (from the origin, defined as the place where ROSplane initialized) or GNSS coordinates given in LLA (latitude, longitude, and altitude).
+The GNSS coordinates are converted to NED coordinates by `path_planner` before sending them to `path_manager`.
+
+The `lla` field is a flag that tells `path_planner` if the coordinates provided are in the NED or world (GNSS) frame.
+Set it to `true` if the coordinates for the waypoint are given in GNSS coordinates.
+Note that GNSS and relative waypoints can be used together, meaning that one waypoint could be in the NED frame while the next one is in the global frame, since ROSplane converts global coordinates to the NED frame before publishing them to `path_manager`. 
+
+!!! note
+    Make sure that the `lla` field is set correctly if you decide to use GNSS coordinates, or your waypoints will be incorrect.
+
+The `chi_d` field controls the desired course at the waypoint.
+Note that this is only used if the `use_chi` flag is set to `true`. 
+
+The `use_chi` field determines whether or not the `path_manager` should pay attention to the desired course (`chi_d`). 
+If `use_chi` is set to `true` then `path_manager` will use a Dubins path framework to manage the waypoint.
+If `use_chi` is set to `false`, then `path_manager` will ignore the desired course and will intead use straight lines and fillets to manage the transitions between waypoints.
+See the [Path Manager](./path-manager.md) documentation for more information on this behavior.
+
+The `clear_wp_list` is used internally by `path_planner` when the `/clear_waypoints` service is called.
+It is recommended to use the service call instead of using this field manually.
+
 !!! warning
-    The `w`, `va_d`, and `lla` fields must be valid for every waypoint message sent.
+    The `header`, `w`, `va_d`, and `lla` fields must be valid for every waypoint message sent.
     Failure to add these fields might crash the path planner or your aircraft.
 
 ### List of ROS2 Interfaces
@@ -42,5 +77,11 @@ See the [Parameter Management](../parameter-management.md) page for more details
 | `num_waypoints_to_publish_at_start` | Number of waypoints to immediately publish at launch. If no waypoints are added, it will not publish any. | int | $\geq$ 0 |
 
 ## Recommended Usage
-We recommend using a YAML file and the `/load_mission_from_file` service to load and fly waypoints.
-See the example mission YAML file located at `/path/to/rosplane/rosplane/params/fixedwing_mission.yaml`.
+We recommend using a YAML file and the `/load_mission_from_file` service from the command line to load and fly waypoints.
+See the example mission YAML file located at `/path/to/rosplane/rosplane/params/fixedwing_mission.yaml`.
+
+To call the service, run
+```bash
+ros2 service call /load_mission_from_file rosflight_msgs::srv::ParamFile "{filename: <FILENAME>}"
+```
+where `<FILENAME>` is the path to the mission YAML file.

docs/user-guide/rosplane-setup.md

@@ -154,7 +154,7 @@ 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.
-See [Path Planning](../developer-guide/rosplane/path-planning/path-planning-overview.md) for more information on how to use and tune the path planner, manager, and follower.
+See [Path Planning](../developer-guide/rosplane/navigation/navigation-overview.md) for more information on how to use and tune the path planner, manager, and follower.
 
 ### Adding waypoints
 

mkdocs.yml

@@ -78,11 +78,11 @@ nav:
         - Successive Loop Closure Controller Outline: developer-guide/rosplane/controller/controller-outline.md
         - Total Energy Controller: developer-guide/rosplane/controller/controller-total-energy.md
       - Parameter Management: developer-guide/rosplane/parameter-management.md
-      - Path Planning: 
-        - Path Planning Overview: developer-guide/rosplane/path-planning/path-planning-overview.md
-        - Path Planner: developer-guide/rosplane/path-planning/path-planner.md
-        - Path Manager: developer-guide/rosplane/path-planning/path-manager.md
-        - Path Follower: developer-guide/rosplane/path-planning/path-follower.md
+      - Navigation: 
+        - Path Planning Overview: developer-guide/rosplane/navigation/navigation-overview.md
+        - Path Planner: developer-guide/rosplane/navigation/path-planner.md
+        - Path Manager: developer-guide/rosplane/navigation/path-manager.md
+        - Path Follower: developer-guide/rosplane/navigation/path-follower.md
 #    - ROS Packages:
 #    - ROSplane:
 #    - ROScopter: