Merge in navigation doxygen

rosplane/include/param_manager/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 <jacobmoor2@gmail.com>
 */
@@ -26,38 +26,81 @@ 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);
 
   /**
    * 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();
 
   /**
    * This function sets a previously declared parameter to a new value in both the parameter object
    * 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);
 
   /**
@@ -77,6 +120,5 @@ class param_manager
   rclcpp::Node * container_node_;
 };
 
-
 }   // namespace rosplane
-#endif // PARAM_MANAGER_H
+#endif // PARAM_MANAGER_H

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<rosplane_msgs::msg::State>::SharedPtr vehicle_state_sub_;
+
+  /** 
+   * Subscribes to the current_path topic from the path manager
+   */
   rclcpp::Subscription<rosplane_msgs::msg::CurrentPath>::SharedPtr current_path_sub_;
 
+  /**
+   * Publishes commands to the controller
+   */
   rclcpp::Publisher<rosplane_msgs::msg::ControllerCommands>::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<rclcpp::Parameter> & 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();
 };

rosplane/include/path_manager_base.hpp

@@ -14,14 +14,14 @@
 #include <math.h>
 #include <rclcpp/rclcpp.hpp>
 #include <rosplane_msgs/msg/current_path.hpp>
-#include <rosplane_msgs/msg/state.hpp> // src/rosplane_msgs/msg/State.msg
+#include <rosplane_msgs/msg/state.hpp> 
 #include <rosplane_msgs/msg/waypoint.hpp>
 #include <sensor_msgs/msg/fluid_pressure.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <std_msgs/msg/float32.hpp>
 #include <std_msgs/msg/float32_multi_array.hpp>
 #include <param_manager.hpp>
-//#include <rosplane/ControllerConfig.hpp>!!!
+
 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<waypoint_s> waypoints_;
+  std::vector<waypoint_s> 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<rclcpp::Parameter> & parameters);
 

rosplane/include/path_manager_example.hpp

@@ -3,12 +3,13 @@
 
 #include "path_manager_base.hpp"
 #include <Eigen/Eigen>
-//#include <lib/mathlib/mathlib.h>
 
 #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<std::chrono::system_clock> 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();
 };