diff --git a/.clang-format b/.clang-format index 536a180..b1b6f0b 100644 --- a/.clang-format +++ b/.clang-format @@ -8,12 +8,6 @@ AlignOperands: DontAlign AllowAllArgumentsOnNextLine: false AllowAllConstructorInitializersOnNextLine: false AllowAllParametersOfDeclarationOnNextLine: false -AllowShortBlocksOnASingleLine: Always -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: All -AllowShortIfStatementsOnASingleLine: Always -AllowShortLambdasOnASingleLine: All -AllowShortLoopsOnASingleLine: true AlwaysBreakAfterReturnType: None AlwaysBreakTemplateDeclarations: Yes BreakBeforeBraces: Custom diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 39f7770..6e68096 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -64,7 +64,6 @@ class ControllerBase : public rclcpp::Node float get_theta_c() { return controller_commands_.theta_c; }; protected: - /** * This struct holds all of the inputs to the control algorithm. */ @@ -90,12 +89,12 @@ class ControllerBase : public rclcpp::Node */ struct Output { - float theta_c; /**< The commanded pitch angle from the altitude control loop */ - float phi_c; /**< The commanded roll angle from the course control loop */ - float delta_e; /**< The commanded elevator deflection */ - float delta_a; /**< The commanded aileron deflection */ - float delta_r; /**< The commanded rudder deflection */ - float delta_t; /**< The commanded throttle deflection */ + float theta_c; /**< The commanded pitch angle from the altitude control loop */ + float phi_c; /**< The commanded roll angle from the course control loop */ + float delta_e; /**< The commanded elevator deflection */ + float delta_a; /**< The commanded aileron deflection */ + float delta_r; /**< The commanded rudder deflection */ + float delta_t; /**< The commanded throttle deflection */ AltZones current_zone; /**< The current altitude zone for the control */ }; @@ -136,7 +135,7 @@ class ControllerBase : public rclcpp::Node * This timer controls how often commands are published by the autopilot. */ rclcpp::TimerBase::SharedPtr timer_; - + /** * Period of the timer that controlls how often commands are published. */ diff --git a/rosplane/include/controller_state_machine.hpp b/rosplane/include/controller_state_machine.hpp index f58f43a..9f0dbcf 100644 --- a/rosplane/include/controller_state_machine.hpp +++ b/rosplane/include/controller_state_machine.hpp @@ -17,8 +17,7 @@ class ControllerStateMachine : public ControllerBase * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void control(const Input & input, - Output & output); + virtual void control(const Input & input, Output & output); protected: /** @@ -31,24 +30,21 @@ class ControllerStateMachine : public ControllerBase * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void take_off(const Input & input, - Output & output) = 0; + virtual void take_off(const Input & input, Output & output) = 0; /** * This function continually loops while the aircraft is in the climb zone. It is implemented by the child. * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void climb(const Input & input, - Output & output) = 0; + virtual void climb(const Input & input, Output & output) = 0; /** * This function continually loops while the aircraft is in the altitude hold zone. It is implemented by the child. * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void altitude_hold(const Input & input, - Output & output) = 0; + virtual void altitude_hold(const Input & input, Output & output) = 0; /** * This function runs when the aircraft exits the take-off zone this is often used to reset integrator values. It is diff --git a/rosplane/include/controller_successive_loop.hpp b/rosplane/include/controller_successive_loop.hpp index 014e263..f9dc12e 100644 --- a/rosplane/include/controller_successive_loop.hpp +++ b/rosplane/include/controller_successive_loop.hpp @@ -21,8 +21,7 @@ class ControllerSucessiveLoop : public ControllerStateMachine * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void take_off(const Input & input, - Output & output); + virtual void take_off(const Input & input, Output & output); /** * This function continually loops while the aircraft is in the climb zone. The lateral and longitudinal control @@ -30,8 +29,7 @@ class ControllerSucessiveLoop : public ControllerStateMachine * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void climb(const Input & input, - Output & output); + virtual void climb(const Input & input, Output & output); /** * This function continually loops while the aircraft is in the altitude hold zone. The lateral and longitudinal @@ -39,8 +37,7 @@ class ControllerSucessiveLoop : public ControllerStateMachine * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void altitude_hold(const Input & input, - Output & output); + virtual void altitude_hold(const Input & input, Output & output); /** * This function runs when the aircraft exits the take-off zone. Any changes to the controller that need to happen @@ -73,16 +70,14 @@ class ControllerSucessiveLoop : public ControllerStateMachine * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void alt_hold_longitudinal_control(const Input & input, - Output & output); + virtual void alt_hold_longitudinal_control(const Input & input, Output & output); /** * This function runs the lateral control loops for the climb zone. * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void climb_lateral_control(const Input & input, - Output & output); + virtual void climb_lateral_control(const Input & input, Output & output); /** * This function runs the longitudinal control loops for the climb zone. @@ -103,8 +98,7 @@ class ControllerSucessiveLoop : public ControllerStateMachine * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void take_off_longitudinal_control(const Input & input, - Output & output); + virtual void take_off_longitudinal_control(const Input & input, Output & output); /** * The control loop for moving to and holding a commanded course. @@ -219,7 +213,7 @@ class ControllerSucessiveLoop : public ControllerStateMachine float delta_r_delay_; float r_delay_; - /** + /** * Saturate a given value to a maximum or minimum of the limits. * @param value The value to saturate. * @param up_limit The maximum the value can take on. diff --git a/rosplane/include/controller_total_energy.hpp b/rosplane/include/controller_total_energy.hpp index 85eea85..96220a4 100644 --- a/rosplane/include/controller_total_energy.hpp +++ b/rosplane/include/controller_total_energy.hpp @@ -20,8 +20,7 @@ class ControllerTotalEnergy : public ControllerSucessiveLoop * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void take_off_longitudinal_control(const Input & input, - Output & output); + virtual void take_off_longitudinal_control(const Input & input, Output & output); /** * This function overrides the longitudinal control loops for the climb zone. @@ -35,8 +34,7 @@ class ControllerTotalEnergy : public ControllerSucessiveLoop * @param input The command inputs to the controller such as course and airspeed. * @param output The control efforts calculated and selected intermediate values. */ - virtual void alt_hold_longitudinal_control(const Input & input, - Output & output); + virtual void alt_hold_longitudinal_control(const Input & input, Output & output); /** * This function overrides when the aircraft exits the take-off zone. Any changes to the controller that need to happen diff --git a/rosplane/include/estimator_continuous_discrete.hpp b/rosplane/include/estimator_continuous_discrete.hpp index 2bc654f..5fb854a 100644 --- a/rosplane/include/estimator_continuous_discrete.hpp +++ b/rosplane/include/estimator_continuous_discrete.hpp @@ -41,38 +41,57 @@ class EstimatorContinuousDiscrete : public EstimatorEKF float Vwhat_; float phihat_; float thetahat_; - float psihat_; // TODO: link to an inital condiditons param - - - Eigen::VectorXf attitude_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements); - std::function attitude_dynamics_model; - - Eigen::MatrixXf attitude_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates); - std::function attitude_jacobian_model; - - Eigen::MatrixXf attitude_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates); - std::function attitude_input_jacobian_model; - - Eigen::VectorXf attitude_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs); - std::function attitude_measurement_model; - - Eigen::MatrixXf attitude_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs); - std::function attitude_measurement_jacobian_model; - - Eigen::VectorXf position_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements); - std::function position_dynamics_model; - - Eigen::MatrixXf position_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements); - std::function position_jacobian_model; - - Eigen::MatrixXf position_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs); - std::function position_input_jacobian_model; - - Eigen::VectorXf position_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& input); - std::function position_measurement_model; - - Eigen::MatrixXf position_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& input); - std::function position_measurement_jacobian_model; + float psihat_; // TODO: link to an inital condiditons param + + Eigen::VectorXf attitude_dynamics(const Eigen::VectorXf & state, + const Eigen::VectorXf & measurements); + std::function + attitude_dynamics_model; + + Eigen::MatrixXf attitude_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & angular_rates); + std::function + attitude_jacobian_model; + + Eigen::MatrixXf attitude_input_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & angular_rates); + std::function + attitude_input_jacobian_model; + + Eigen::VectorXf attitude_measurement_prediction(const Eigen::VectorXf & state, + const Eigen::VectorXf & inputs); + std::function + attitude_measurement_model; + + Eigen::MatrixXf attitude_measurement_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & inputs); + std::function + attitude_measurement_jacobian_model; + + Eigen::VectorXf position_dynamics(const Eigen::VectorXf & state, + const Eigen::VectorXf & measurements); + std::function + position_dynamics_model; + + Eigen::MatrixXf position_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & measurements); + std::function + position_jacobian_model; + + Eigen::MatrixXf position_input_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & inputs); + std::function + position_input_jacobian_model; + + Eigen::VectorXf position_measurement_prediction(const Eigen::VectorXf & state, + const Eigen::VectorXf & input); + std::function + position_measurement_model; + + Eigen::MatrixXf position_measurement_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & input); + std::function + position_measurement_jacobian_model; Eigen::Vector2f xhat_a_; // 2 Eigen::Matrix2f P_a_; // 2x2 @@ -119,7 +138,7 @@ class EstimatorContinuousDiscrete : public EstimatorEKF * @brief Initializes the state covariance matrix with the ROS2 parameters */ void initialize_state_covariances(); -}; +}; } // namespace rosplane diff --git a/rosplane/include/estimator_ekf.hpp b/rosplane/include/estimator_ekf.hpp index 4d66ea7..ba941c5 100644 --- a/rosplane/include/estimator_ekf.hpp +++ b/rosplane/include/estimator_ekf.hpp @@ -19,34 +19,30 @@ class EstimatorEKF : public EstimatorROS EstimatorEKF(); protected: - std::tuple measurement_update(Eigen::VectorXf x, - Eigen::VectorXf inputs, - std::function measurement_model, - Eigen::VectorXf y, - std::function measurement_jacobian, - Eigen::MatrixXf R, - Eigen::MatrixXf P); - - std::tuple propagate_model(Eigen::VectorXf x, - std::function dynamic_model, - std::function jacobian, - Eigen::VectorXf inputs, - std::function input_jacobian, - Eigen::MatrixXf P, - Eigen::MatrixXf Q, - Eigen::MatrixXf Q_g, - float Ts); - - std::tuple single_measurement_update(float measurement, - float mesurement_prediction, - float measurement_uncertainty, - Eigen::VectorXf measurement_jacobian, - Eigen::VectorXf x, - Eigen::MatrixXf P); + std::tuple measurement_update( + Eigen::VectorXf x, Eigen::VectorXf inputs, + std::function measurement_model, + Eigen::VectorXf y, + std::function + measurement_jacobian, + Eigen::MatrixXf R, Eigen::MatrixXf P); + + std::tuple propagate_model( + Eigen::VectorXf x, + std::function dynamic_model, + std::function jacobian, + Eigen::VectorXf inputs, + std::function input_jacobian, + Eigen::MatrixXf P, Eigen::MatrixXf Q, Eigen::MatrixXf Q_g, float Ts); + + std::tuple + single_measurement_update(float measurement, float mesurement_prediction, + float measurement_uncertainty, Eigen::VectorXf measurement_jacobian, + Eigen::VectorXf x, Eigen::MatrixXf P); private: virtual void estimate(const Input & input, Output & output) override = 0; -}; +}; } // namespace rosplane diff --git a/rosplane/include/estimator_ros.hpp b/rosplane/include/estimator_ros.hpp index cda7c8c..f6a9eb8 100644 --- a/rosplane/include/estimator_ros.hpp +++ b/rosplane/include/estimator_ros.hpp @@ -82,15 +82,14 @@ class EstimatorROS : public rclcpp::Node bool baro_init_; /**< Initial barometric pressure */ - virtual void estimate(const Input & input, - Output & output) = 0; + virtual void estimate(const Input & input, Output & output) = 0; ParamManager params_; bool gps_init_; - double init_lat_ = 0.0; /**< Initial latitude in degrees */ - double init_lon_ = 0.0; /**< Initial longitude in degrees */ - float init_alt_ = 0.0; /**< Initial altitude in meters above MSL */ - float init_static_; /**< Initial static pressure (mbar) */ + double init_lat_ = 0.0; /**< Initial latitude in degrees */ + double init_lon_ = 0.0; /**< Initial longitude in degrees */ + float init_alt_ = 0.0; /**< Initial altitude in meters above MSL */ + float init_static_; /**< Initial static pressure (mbar) */ private: rclcpp::Publisher::SharedPtr vehicle_state_pub_; diff --git a/rosplane/include/param_manager/param_manager.hpp b/rosplane/include/param_manager/param_manager.hpp index dda4dca..2268d76 100644 --- a/rosplane/include/param_manager/param_manager.hpp +++ b/rosplane/include/param_manager/param_manager.hpp @@ -15,7 +15,7 @@ namespace rosplane { - + class ParamManager { public: @@ -66,7 +66,7 @@ class ParamManager * 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); + void declare_int(std::string param_name, int64_t value); /** * Helper function to declare parameters in the param_manager object @@ -103,7 +103,7 @@ class ParamManager * and the ROS system. */ void set_string(std::string param_name, std::string value); - + /** * This function should be called in the parametersCallback function in a containing ROS node. * It takes in a vector of changed parameters and updates them within the params_ object. @@ -121,5 +121,5 @@ class ParamManager rclcpp::Node * container_node_; }; -} // namespace rosplane +} // namespace rosplane #endif // PARAM_MANAGER_H diff --git a/rosplane/include/path_follower_base.hpp b/rosplane/include/path_follower_base.hpp index 2282912..7b58aa6 100644 --- a/rosplane/include/path_follower_base.hpp +++ b/rosplane/include/path_follower_base.hpp @@ -52,8 +52,7 @@ class PathFollowerBase : public rclcpp::Node double phi_ff; /** feed forward term for orbits (rad) */ }; - virtual void follow(const Input & input, - Output & output) = 0; + virtual void follow(const Input & input, Output & output) = 0; ParamManager params_; @@ -72,7 +71,7 @@ class PathFollowerBase : public rclcpp::Node * Publishes commands to the controller */ rclcpp::Publisher::SharedPtr controller_commands_pub_; - + std::chrono::microseconds timer_period_; rclcpp::TimerBase::SharedPtr update_timer_; diff --git a/rosplane/include/path_follower_example.hpp b/rosplane/include/path_follower_example.hpp index 6755e33..d089ce0 100644 --- a/rosplane/include/path_follower_example.hpp +++ b/rosplane/include/path_follower_example.hpp @@ -12,8 +12,7 @@ class PathFollowerExample : public PathFollowerBase PathFollowerExample(); private: - virtual void follow(const Input & input, - Output & output); + virtual void follow(const Input & input, Output & output); }; } // namespace rosplane diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp index 1ebb3a2..96c13e2 100644 --- a/rosplane/include/path_manager_base.hpp +++ b/rosplane/include/path_manager_base.hpp @@ -29,7 +29,6 @@ class PathManagerBase : public rclcpp::Node PathManagerBase(); protected: - struct Waypoint { float w[3]; @@ -64,7 +63,7 @@ class PathManagerBase : public rclcpp::Node int8_t lamda; /** Direction of orbital path (cw is 1, ccw is -1) */ }; - ParamManager params_; /** Holds the parameters for the path_manager and children */ + ParamManager params_; /** Holds the parameters for the path_manager and children */ /** * @brief Manages the current path based on the stored waypoint list @@ -72,8 +71,7 @@ class PathManagerBase : public rclcpp::Node * @param input: Input object that contains information about the waypoint * @param output: Output object that contains the parameters for the desired type of line, based on the current and next waypoints */ - virtual void manage(const Input & input, - Output & output) = 0; + virtual void manage(const Input & input, Output & output) = 0; private: rclcpp::Subscription::SharedPtr @@ -91,10 +89,11 @@ class PathManagerBase : public rclcpp::Node rclcpp::TimerBase::SharedPtr update_timer_; OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - 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 */ - + 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 */ /** * @brief Callback that gets triggered when a ROS2 parameter is changed diff --git a/rosplane/include/path_manager_example.hpp b/rosplane/include/path_manager_example.hpp index 3eea5ba..d254414 100644 --- a/rosplane/include/path_manager_example.hpp +++ b/rosplane/include/path_manager_example.hpp @@ -36,12 +36,11 @@ class PathManagerExample : public PathManagerBase private: std::chrono::time_point start_time_; FilletState fil_state_; - + /** * @brief Determines the line type and calculates the line parameters to publish to path_follower */ - virtual void manage(const Input & input, - Output & output); + virtual void manage(const Input & input, Output & output); /** * @brief Calculates the most convenient orbit direction based on the orientation of the vehicle relative to the orbit center @@ -64,7 +63,8 @@ class PathManagerExample : public PathManagerBase * @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 Input & input, Output & output); + void increment_indices(int & idx_a, int & idx_b, int & idx_c, const Input & input, + Output & output); /** * @brief Manages a straight line segment. Calculates the appropriate line parameters to send to the path follower @@ -72,8 +72,7 @@ class PathManagerExample : public PathManagerBase * @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 Input & input, - Output & output); + void manage_line(const Input & input, Output & output); /** * @brief Manages a fillet line segment. Calculates the appropriate line parameters to send to the path follower @@ -81,8 +80,7 @@ class PathManagerExample : public PathManagerBase * @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 Input & input, - Output & output); + void manage_fillet(const Input & input, Output & output); /** * @brief Manages a Dubins path segment. Calculates the appropriate line parameters to send to the path follower @@ -90,8 +88,7 @@ class PathManagerExample : public PathManagerBase * @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 Input & input, - Output & output); + void manage_dubins(const Input & input, Output & output); DubinState dub_state_; @@ -123,8 +120,7 @@ class PathManagerExample : public PathManagerBase * @param end_node: Ending waypoint of the Dubins path * @param R: Minimum turning radius R */ - void dubins_parameters(const Waypoint start_node, const Waypoint end_node, - float R); + void dubins_parameters(const Waypoint start_node, const Waypoint end_node, float R); /** * @brief Computes the rotation matrix for a rotation in the z plane (normal to the Dubins plane) diff --git a/rosplane/include/path_planner.hpp b/rosplane/include/path_planner.hpp index 81140e7..ffb2208 100644 --- a/rosplane/include/path_planner.hpp +++ b/rosplane/include/path_planner.hpp @@ -4,9 +4,9 @@ #include #include "param_manager.hpp" -#include "rosplane_msgs/srv/add_waypoint.hpp" #include "rosplane_msgs/msg/state.hpp" #include "rosplane_msgs/msg/waypoint.hpp" +#include "rosplane_msgs/srv/add_waypoint.hpp" #define EARTH_RADIUS 6378145.0f @@ -19,7 +19,7 @@ class PathPlanner : public rclcpp::Node PathPlanner(); ~PathPlanner(); - ParamManager params_; /** Holds the parameters for the path_planner*/ + ParamManager params_; /** Holds the parameters for the path_planner*/ private: /** @@ -88,7 +88,8 @@ class PathPlanner : public rclcpp::Node * @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(); + const std_srvs::srv::Trigger::Response::SharedPtr & res); + void clear_path(); /** * @brief "print_path" service callback. Prints waypoints to the terminal @@ -119,7 +120,7 @@ class PathPlanner : public rclcpp::Node * * @return True if loading waypoints was successful, false otherwise */ - bool load_mission_from_file(const std::string& filename); + bool load_mission_from_file(const std::string & filename); /** * @brief Callback for the rosplane_msgs::msg::State publisher. Saves the initial GNSS coordinates @@ -170,4 +171,4 @@ class PathPlanner : public rclcpp::Node */ std::vector wps; }; -} // namespace rosplane \ No newline at end of file +} // namespace rosplane \ No newline at end of file diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index 6a74a0b..7f23332 100644 --- a/rosplane/src/controller_base.cpp +++ b/rosplane/src/controller_base.cpp @@ -11,13 +11,15 @@ namespace rosplane { ControllerBase::ControllerBase() - : Node("controller_base"), params_(this), params_initialized_(false) + : Node("controller_base") + , params_(this) + , params_initialized_(false) { // Advertise published topics. actuators_pub_ = this->create_publisher("command", 10); - controller_internals_pub_ = this->create_publisher( - "controller_internals", 10); + controller_internals_pub_ = + this->create_publisher("controller_internals", 10); // Advertise subscribed topics and set bound callbacks. controller_commands_sub_ = this->create_subscription( @@ -148,18 +150,17 @@ ControllerBase::parametersCallback(const std::vector & parame { 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 does not is not a parameter of the controller node."; bool success = params_.set_parameters_callback(parameters); - if (success) - { + if (success) { result.successful = true; result.reason = "success"; } if (params_initialized_ && success) { - std::chrono::microseconds curr_period = std::chrono::microseconds(static_cast(1.0 / params_.get_double("controller_output_frequency") * 1'000'000)); + std::chrono::microseconds curr_period = std::chrono::microseconds( + static_cast(1.0 / params_.get_double("controller_output_frequency") * 1'000'000)); if (timer_period_ != curr_period) { timer_->cancel(); set_timer(); @@ -173,13 +174,11 @@ void ControllerBase::set_timer() { double frequency = params_.get_double("controller_output_frequency"); - timer_period_ = - std::chrono::microseconds(static_cast(1.0 / frequency * 1'000'000)); + timer_period_ = std::chrono::microseconds(static_cast(1.0 / frequency * 1'000'000)); // Set timer to trigger bound callback (actuator_controls_publish) at the given periodicity. timer_ = this->create_wall_timer(timer_period_, - std::bind(&ControllerBase::actuator_controls_publish, - this)); + std::bind(&ControllerBase::actuator_controls_publish, this)); } void ControllerBase::convert_to_pwm(Output & output) @@ -192,7 +191,7 @@ void ControllerBase::convert_to_pwm(Output & output) // Multiply each control effort (in radians) by a scaling factor to a pwm. // TODO investigate why this is named "pwm". The actual scaling to pwm happens in rosflight_io. - output.delta_e = output.delta_e * pwm_rad_e; + output.delta_e = output.delta_e * pwm_rad_e; output.delta_a = output.delta_a * pwm_rad_a; output.delta_r = output.delta_r * pwm_rad_r; } diff --git a/rosplane/src/controller_state_machine.cpp b/rosplane/src/controller_state_machine.cpp index 5f2facd..4c4cb58 100644 --- a/rosplane/src/controller_state_machine.cpp +++ b/rosplane/src/controller_state_machine.cpp @@ -16,10 +16,9 @@ ControllerStateMachine::ControllerStateMachine() params_.set_parameters(); } -void ControllerStateMachine::control(const Input & input, - Output & output) +void ControllerStateMachine::control(const Input & input, Output & output) { - + // For readability, declare parameters that will be used in this controller double alt_toz = params_.get_double("alt_toz"); double alt_hz = params_.get_double("alt_hz"); diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index dbf078e..5036b57 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/rosplane/src/controller_successive_loop.cpp @@ -40,8 +40,7 @@ void ControllerSucessiveLoop::take_off_exit() // Put any code that should run as the airplane exits take off mode. } -void ControllerSucessiveLoop::climb(const Input & input, - Output & output) +void ControllerSucessiveLoop::climb(const Input & input, Output & output) { // Run lateral and longitudinal controls. climb_lateral_control(input, output); @@ -59,8 +58,7 @@ void ControllerSucessiveLoop::climb_exit() a_differentiator_ = 0; } -void ControllerSucessiveLoop::altitude_hold(const Input & input, - Output & output) +void ControllerSucessiveLoop::altitude_hold(const Input & input, Output & output) { // Run lateral and longitudinal controls. alt_hold_lateral_control(input, output); @@ -73,11 +71,10 @@ void ControllerSucessiveLoop::altitude_hold_exit() c_integrator_ = 0; } -void ControllerSucessiveLoop::alt_hold_lateral_control(const Input & input, - Output & output) +void ControllerSucessiveLoop::alt_hold_lateral_control(const Input & input, Output & output) { // For readability, declare parameters here that will be used in this function - bool roll_override = params_.get_bool("roll_command_override"); // Declared in controller_base + bool roll_override = params_.get_bool("roll_command_override"); // Declared in controller_base // Set rudder command to zero, can use coordinated_turn_hold if implemented. // Find commanded roll angle in order to achieve commanded course. @@ -85,17 +82,18 @@ void ControllerSucessiveLoop::alt_hold_lateral_control(const Input & input, output.delta_r = yaw_damper(input.r); //coordinated_turn_hold(input.beta, params) output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r); - if (roll_override) { output.phi_c = get_phi_c(); } + if (roll_override) { + output.phi_c = get_phi_c(); + } output.delta_a = roll_hold(output.phi_c, input.phi, input.p); } -void ControllerSucessiveLoop::alt_hold_longitudinal_control(const Input & input, - Output & output) +void ControllerSucessiveLoop::alt_hold_longitudinal_control(const Input & input, Output & output) { // For readability, declare parameters here that will be used in this function - double alt_hz = params_.get_double("alt_hz"); // Declared in controller_state_machine - bool pitch_override = params_.get_bool("pitch_command_override"); // Declared in controller_base + double alt_hz = params_.get_double("alt_hz"); // Declared in controller_state_machine + bool pitch_override = params_.get_bool("pitch_command_override"); // Declared in controller_base // Saturate the altitude command. double adjusted_hc = adjust_h_c(input.h_c, input.h, alt_hz); @@ -104,13 +102,14 @@ void ControllerSucessiveLoop::alt_hold_longitudinal_control(const Input & input, output.delta_t = airspeed_with_throttle_hold(input.va_c, input.va); output.theta_c = altitude_hold_control(adjusted_hc, input.h); - if (pitch_override) { output.theta_c = get_theta_c(); } + if (pitch_override) { + output.theta_c = get_theta_c(); + } output.delta_e = pitch_hold(output.theta_c, input.theta, input.q); } -void ControllerSucessiveLoop::climb_lateral_control(const Input & input, - Output & output) +void ControllerSucessiveLoop::climb_lateral_control(const Input & input, Output & output) { // Maintain straight flight while gaining altitude. output.phi_c = 0; @@ -118,11 +117,10 @@ void ControllerSucessiveLoop::climb_lateral_control(const Input & input, output.delta_r = yaw_damper(input.r); } -void ControllerSucessiveLoop::climb_longitudinal_control(const Input & input, - Output & output) +void ControllerSucessiveLoop::climb_longitudinal_control(const Input & input, Output & output) { // For readability, declare parameters here that will be used in this function - double alt_hz = params_.get_double("alt_hz"); // Declared in controller_state_machine + double alt_hz = params_.get_double("alt_hz"); // Declared in controller_state_machine // Saturate the altitude command. double adjusted_hc = adjust_h_c(input.h_c, input.h, alt_hz); @@ -133,8 +131,7 @@ void ControllerSucessiveLoop::climb_longitudinal_control(const Input & input, output.delta_e = pitch_hold(output.theta_c, input.theta, input.q); } -void ControllerSucessiveLoop::take_off_lateral_control(const Input & input, - Output & output) +void ControllerSucessiveLoop::take_off_lateral_control(const Input & input, Output & output) { // In the take-off zone maintain level straight flight by commanding a roll angle of 0 and rudder of 0. output.delta_r = 0.0; @@ -142,19 +139,17 @@ void ControllerSucessiveLoop::take_off_lateral_control(const Input & input, output.delta_a = roll_hold(output.phi_c, input.phi, input.p); } -void ControllerSucessiveLoop::take_off_longitudinal_control(const Input & input, - Output & output) +void ControllerSucessiveLoop::take_off_longitudinal_control(const Input & input, Output & output) { // For readability, declare parameters here that will be used in this function double max_takeoff_throttle = params_.get_double("max_takeoff_throttle"); double cmd_takeoff_pitch = params_.get_double("cmd_takeoff_pitch"); - + // Set throttle to not overshoot altitude. - output.delta_t = - sat(airspeed_with_throttle_hold(input.va_c, input.va), max_takeoff_throttle, 0); + output.delta_t = sat(airspeed_with_throttle_hold(input.va_c, input.va), max_takeoff_throttle, 0); // Command a shallow pitch angle to gain altitude. - output.theta_c = cmd_takeoff_pitch * M_PI / 180.0; + output.theta_c = cmd_takeoff_pitch * M_PI / 180.0; output.delta_e = pitch_hold(output.theta_c, input.theta, input.q); } @@ -202,7 +197,8 @@ void ControllerSucessiveLoop::take_off_longitudinal_control(const Input & input, float ControllerSucessiveLoop::course_hold(float chi_c, float chi, float phi_ff, float r) { // For readability, declare parameters here that will be used in this function - double frequency = params_.get_double("controller_output_frequency"); // Declared in controller_base + double frequency = + params_.get_double("controller_output_frequency"); // Declared in controller_base double c_kp = params_.get_double("c_kp"); double c_ki = params_.get_double("c_ki"); double c_kd = params_.get_double("c_kd"); @@ -221,8 +217,7 @@ float ControllerSucessiveLoop::course_hold(float chi_c, float chi, float phi_ff, float ui = c_ki * c_integrator_; float ud = c_kd * r; - float phi_c = - sat(up + ui + ud + phi_ff, max_roll * M_PI / 180.0, -max_roll * M_PI / 180.0); + float phi_c = sat(up + ui + ud + phi_ff, max_roll * M_PI / 180.0, -max_roll * M_PI / 180.0); float phi_c_unsat = up + ui + ud + phi_ff; if (fabs(phi_c - phi_c_unsat) > 0.0001) { // If the controller is saturating don't integrate. @@ -236,13 +231,14 @@ float ControllerSucessiveLoop::course_hold(float chi_c, float chi, float phi_ff, float ControllerSucessiveLoop::roll_hold(float phi_c, float phi, float p) { // For readability, declare parameters here that will be used in this function - double frequency = params_.get_double("controller_output_frequency"); // Declared in controller_base + double frequency = + params_.get_double("controller_output_frequency"); // Declared in controller_base double r_kp = params_.get_double("r_kp"); double r_ki = params_.get_double("r_ki"); double r_kd = params_.get_double("r_kd"); double max_a = params_.get_double("max_a"); double trim_a = params_.get_double("trim_a"); - double pwm_rad_a = params_.get_double("pwm_rad_a"); // Declared in controller base + double pwm_rad_a = params_.get_double("pwm_rad_a"); // Declared in controller base float error = phi_c - phi; @@ -269,13 +265,14 @@ float ControllerSucessiveLoop::roll_hold(float phi_c, float phi, float p) float ControllerSucessiveLoop::pitch_hold(float theta_c, float theta, float q) { // For readability, declare parameters here that will be used in this function - double frequency = params_.get_double("controller_output_frequency"); // Declared in controller_base + double frequency = + params_.get_double("controller_output_frequency"); // Declared in controller_base double p_kp = params_.get_double("p_kp"); double p_ki = params_.get_double("p_ki"); double p_kd = params_.get_double("p_kd"); double max_e = params_.get_double("max_e"); double trim_e = params_.get_double("trim_e"); - double pwm_rad_e = params_.get_double("pwm_rad_e"); // Declared in controller_base + double pwm_rad_e = params_.get_double("pwm_rad_e"); // Declared in controller_base float error = theta_c - theta; @@ -302,7 +299,8 @@ float ControllerSucessiveLoop::pitch_hold(float theta_c, float theta, float q) float ControllerSucessiveLoop::airspeed_with_throttle_hold(float va_c, float va) { // For readability, declare parameters here that will be used in this function - double frequency = params_.get_double("controller_output_frequency"); // Declared in controller_base + double frequency = + params_.get_double("controller_output_frequency"); // Declared in controller_base double tau = params_.get_double("tau"); double a_t_kp = params_.get_double("a_t_kp"); double a_t_ki = params_.get_double("a_t_ki"); @@ -338,7 +336,8 @@ float ControllerSucessiveLoop::airspeed_with_throttle_hold(float va_c, float va) float ControllerSucessiveLoop::altitude_hold_control(float h_c, float h) { // For readability, declare parameters here that will be used in this function - double frequency = params_.get_double("controller_output_frequency"); // Declared in controller_base + double frequency = + params_.get_double("controller_output_frequency"); // Declared in controller_base double alt_hz = params_.get_double("alt_hz"); double tau = params_.get_double("tau"); double a_kp = params_.get_double("a_kp"); @@ -376,12 +375,13 @@ float ControllerSucessiveLoop::altitude_hold_control(float h_c, float h) float ControllerSucessiveLoop::yaw_damper(float r) { - double frequency = params_.get_double("controller_output_frequency"); // Declared in controller_base + double frequency = + params_.get_double("controller_output_frequency"); // Declared in controller_base float y_pwo = params_.get_double("y_pwo"); float y_kr = params_.get_double("y_kr"); float max_r = params_.get_double("max_r"); - float Ts = 1.0/frequency; + float Ts = 1.0 / frequency; float n0 = 0.0; float n1 = 1.0; @@ -412,14 +412,15 @@ float ControllerSucessiveLoop::sat(float value, float up_limit, float low_limit) // Set to upper limit if larger than that limit. // Set to lower limit if smaller than that limit. // Otherwise, do not change the value. - - if (up_limit < 0.0) - { - RCLCPP_WARN_ONCE(this->get_logger(), "WARNING: Upper limit in saturation function is negative."); + + if (up_limit < 0.0) { + RCLCPP_WARN_ONCE(this->get_logger(), + "WARNING: Upper limit in saturation function is negative."); } float rVal; - if (value > up_limit) rVal = up_limit; + if (value > up_limit) + rVal = up_limit; else if (value < low_limit) rVal = low_limit; else diff --git a/rosplane/src/controller_total_energy.cpp b/rosplane/src/controller_total_energy.cpp index 2f90b96..39168df 100644 --- a/rosplane/src/controller_total_energy.cpp +++ b/rosplane/src/controller_total_energy.cpp @@ -17,16 +17,16 @@ ControllerTotalEnergy::ControllerTotalEnergy() params_.set_parameters(); } -void ControllerTotalEnergy::take_off_longitudinal_control(const Input & input, - Output & output) +void ControllerTotalEnergy::take_off_longitudinal_control(const Input & input, Output & output) { // For readability, declare parameters here that will be used in this function double max_takeoff_throttle = params_.get_double("max_takeoff_throttle"); - double cmd_takeoff_pitch = params_.get_double("cmd_takeoff_pitch"); // Declared in controller_successive_loop + double cmd_takeoff_pitch = + params_.get_double("cmd_takeoff_pitch"); // Declared in controller_successive_loop // Set throttle to not overshoot altitude. - output.delta_t = sat(total_energy_throttle(input.va_c, input.va, input.h_c, input.h), - max_takeoff_throttle, 0); + output.delta_t = + sat(total_energy_throttle(input.va_c, input.va, input.h_c, input.h), max_takeoff_throttle, 0); // Command a shallow pitch angle to gain altitude. output.theta_c = cmd_takeoff_pitch * M_PI / 180.0; @@ -44,8 +44,7 @@ void ControllerTotalEnergy::take_off_exit() // Place any controller code that should run as you exit the take-off regime here. } -void ControllerTotalEnergy::climb_longitudinal_control(const Input & input, - Output & output) +void ControllerTotalEnergy::climb_longitudinal_control(const Input & input, Output & output) { // For readability, declare parameters here that will be used in this function double alt_hz = params_.get_double("alt_hz"); @@ -68,8 +67,7 @@ void ControllerTotalEnergy::climb_exit() // Place any controller code that should run as you exit the climb regime here. } -void ControllerTotalEnergy::alt_hold_longitudinal_control(const Input & input, - Output & output) +void ControllerTotalEnergy::alt_hold_longitudinal_control(const Input & input, Output & output) { // For readability, declare parameters here that will be used in this function double alt_hz = params_.get_double("alt_hz"); @@ -101,7 +99,7 @@ float ControllerTotalEnergy::total_energy_throttle(float va_c, float va, float h double e_ki = params_.get_double("e_ki"); double e_kd = params_.get_double("e_kd"); double max_t = params_.get_double("max_t"); // Declared in controller_successive_loop - double trim_t = params_.get_double("trim_t"); // Declared in controller_successive_loop + double trim_t = params_.get_double("trim_t"); // Declared in controller_successive_loop // Update energies based off of most recent data. update_energies(va_c, va, h_c, h); @@ -117,11 +115,12 @@ float ControllerTotalEnergy::total_energy_throttle(float va_c, float va, float h E_error_prev_ = E_error; // TODO: Add this to params - if (h < .5) { E_integrator_ = 0; } + if (h < .5) { + E_integrator_ = 0; + } // Return saturated throttle command. - return sat(e_kp * E_error + e_ki * E_integrator_, max_t, 0.0) - + trim_t; + return sat(e_kp * E_error + e_ki * E_integrator_, max_t, 0.0) + trim_t; } float ControllerTotalEnergy::total_energy_pitch(float va_c, float va, float h_c, float h) @@ -131,7 +130,7 @@ float ControllerTotalEnergy::total_energy_pitch(float va_c, float va, float h_c, double l_kp = params_.get_double("l_kp"); double l_ki = params_.get_double("l_ki"); double l_kd = params_.get_double("l_kd"); - double max_roll = params_.get_double("max_roll"); // Declared in controller_successive_loop + double max_roll = params_.get_double("max_roll"); // Declared in controller_successive_loop // Update energies based off of most recent data. update_energies(va_c, va, h_c, h); @@ -149,7 +148,7 @@ float ControllerTotalEnergy::total_energy_pitch(float va_c, float va, float h_c, // Return saturated pitch command. return sat(l_kp * L_error + l_ki * L_integrator_, max_roll * M_PI / 180.0, - -max_roll * M_PI / 180.0); + -max_roll * M_PI / 180.0); } void ControllerTotalEnergy::update_energies(float va_c, float va, float h_c, float h) diff --git a/rosplane/src/estimator_continuous_discrete.cpp b/rosplane/src/estimator_continuous_discrete.cpp index 3c964f1..0f6f216 100644 --- a/rosplane/src/estimator_continuous_discrete.cpp +++ b/rosplane/src/estimator_continuous_discrete.cpp @@ -60,7 +60,8 @@ EstimatorContinuousDiscrete::EstimatorContinuousDiscrete() N_ = params_.get_int("num_propagation_steps"); } -EstimatorContinuousDiscrete::EstimatorContinuousDiscrete(bool use_params) : EstimatorContinuousDiscrete() +EstimatorContinuousDiscrete::EstimatorContinuousDiscrete(bool use_params) + : EstimatorContinuousDiscrete() { double init_lat = params_.get_double("init_lat"); double init_long = params_.get_double("init_lon"); @@ -82,7 +83,8 @@ EstimatorContinuousDiscrete::EstimatorContinuousDiscrete(bool use_params) : Esti init_static_ = init_static; } -void EstimatorContinuousDiscrete::initialize_state_covariances() { +void EstimatorContinuousDiscrete::initialize_state_covariances() +{ double pos_n_initial_cov = params_.get_double("pos_n_initial_cov"); double pos_e_initial_cov = params_.get_double("pos_e_initial_cov"); double vg_initial_cov = params_.get_double("vg_initial_cov"); @@ -101,7 +103,8 @@ void EstimatorContinuousDiscrete::initialize_state_covariances() { P_p_(6, 6) = radians(psi_initial_cov); } -void EstimatorContinuousDiscrete::initialize_uncertainties() { +void EstimatorContinuousDiscrete::initialize_uncertainties() +{ double roll_process_noise = params_.get_double("roll_process_noise"); double pitch_process_noise = params_.get_double("pitch_process_noise"); double gyro_process_noise = params_.get_double("gyro_process_noise"); @@ -171,7 +174,7 @@ void EstimatorContinuousDiscrete::estimate(const Input & input, Output & output) float phat = lpf_gyro_x_; float qhat = lpf_gyro_y_; float rhat = lpf_gyro_z_; - + // These states will allow us to propagate our state model for pitch and roll. Eigen::Vector3f angular_rates; angular_rates << phat, qhat, rhat; @@ -191,7 +194,8 @@ void EstimatorContinuousDiscrete::estimate(const Input & input, Output & output) // when the plane isn't moving or moving slowly, the noise in the sensor // will cause the differential pressure to go negative. This will catch // those cases. - if (lpf_diff_ <= 0) lpf_diff_ = 0.000001; + if (lpf_diff_ <= 0) + lpf_diff_ = 0.000001; float vahat = sqrt(2 / rho * lpf_diff_); @@ -199,24 +203,27 @@ void EstimatorContinuousDiscrete::estimate(const Input & input, Output & output) lpf_accel_x_ = alpha_ * lpf_accel_x_ + (1 - alpha_) * input.accel_x; lpf_accel_y_ = alpha_ * lpf_accel_y_ + (1 - alpha_) * input.accel_y; lpf_accel_z_ = alpha_ * lpf_accel_z_ + (1 - alpha_) * input.accel_z; - + // These are the current states that will allow us to predict our measurement. Eigen::Vector4f att_curr_state_info; att_curr_state_info << angular_rates, vahat; - + // The sensor measurements that we can compare to our preditions. Eigen::Vector3f y_att; y_att << lpf_accel_x_, lpf_accel_y_, lpf_accel_z_; // ATTITUDE (ROLL AND PITCH) ESTIMATION // Prediction step - std::tie(P_a_, xhat_a_) = propagate_model(xhat_a_, attitude_dynamics_model, attitude_jacobian_model, angular_rates, - attitude_input_jacobian_model, P_a_, Q_a_, Q_g_, Ts); + std::tie(P_a_, xhat_a_) = + propagate_model(xhat_a_, attitude_dynamics_model, attitude_jacobian_model, angular_rates, + attitude_input_jacobian_model, P_a_, Q_a_, Q_g_, Ts); // Measurement update - std::tie(P_a_, xhat_a_) = measurement_update(xhat_a_, att_curr_state_info, attitude_measurement_model, y_att, attitude_measurement_jacobian_model, // TODO: change these std::ties to just pass by reference. - R_accel_,P_a_); - + std::tie(P_a_, xhat_a_) = measurement_update( + xhat_a_, att_curr_state_info, attitude_measurement_model, y_att, + attitude_measurement_jacobian_model, // TODO: change these std::ties to just pass by reference. + R_accel_, P_a_); + // Check the estimate for errors check_xhat_a(); @@ -226,20 +233,22 @@ void EstimatorContinuousDiscrete::estimate(const Input & input, Output & output) // Implement continous-discrete EKF to estimate pn, pe, chi, Vg, wn, we // Prediction step - + if (fabsf(xhat_p_(2)) < 0.01f) { xhat_p_(2) = 0.01; // prevent divide by zero } - + // These are the state that will allow us to propagate our state model for the position state. Eigen::Vector attitude_states; attitude_states << angular_rates, xhat_a_(0), xhat_a_(1), vahat; - + // POSITION AND COURSE ESTIMATION // Prediction step - std::tie(P_p_, xhat_p_) = propagate_model(xhat_p_, position_dynamics_model, position_jacobian_model, attitude_states, position_input_jacobian_model, P_p_, Q_p_, Eigen::MatrixXf::Zero(7, 7), Ts); - - // Check wrapping of the heading and course. + std::tie(P_p_, xhat_p_) = + propagate_model(xhat_p_, position_dynamics_model, position_jacobian_model, attitude_states, + position_input_jacobian_model, P_p_, Q_p_, Eigen::MatrixXf::Zero(7, 7), Ts); + + // Check wrapping of the heading and course. xhat_p_(3) = wrap_within_180(0.0, xhat_p_(3)); xhat_p_(6) = wrap_within_180(0.0, xhat_p_(6)); if (xhat_p_(3) > radians(180.0f) || xhat_p_(3) < radians(-180.0f)) { @@ -260,13 +269,15 @@ void EstimatorContinuousDiscrete::estimate(const Input & input, Output & output) //wrap course measurement float gps_course = fmodf(input.gps_course, radians(360.0f)); gps_course = wrap_within_180(xhat_p_(3), gps_course); - + // Measurements for the postional states. Eigen::Vector y_pos; y_pos << input.gps_n, input.gps_e, input.gps_Vg, gps_course, 0.0, 0.0; - + // Update the state and covariance with based on the predicted and actual measurements. - std::tie(P_p_, xhat_p_) = measurement_update(xhat_p_, pos_curr_state_info, position_measurement_model, y_pos, position_measurement_jacobian_model, R_p_, P_p_); + std::tie(P_p_, xhat_p_) = + measurement_update(xhat_p_, pos_curr_state_info, position_measurement_model, y_pos, + position_measurement_jacobian_model, R_p_, P_p_); if (xhat_p_(0) > gps_n_lim || xhat_p_(0) < -gps_n_lim) { RCLCPP_WARN(this->get_logger(), "gps n limit reached"); @@ -343,16 +354,18 @@ void EstimatorContinuousDiscrete::estimate(const Input & input, Output & output) output.psi = psihat; } -Eigen::VectorXf EstimatorContinuousDiscrete::attitude_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates) +Eigen::VectorXf +EstimatorContinuousDiscrete::attitude_dynamics(const Eigen::VectorXf & state, + const Eigen::VectorXf & angular_rates) { float cp = cosf(state(0)); // cos(phi) float sp = sinf(state(0)); // sin(phi) float tt = tanf(state(1)); // tan(theta) - + float p = angular_rates(0); float q = angular_rates(1); float r = angular_rates(2); - + Eigen::Vector2f f; f(0) = p + (q * sp + r * cp) * tt; @@ -361,7 +374,8 @@ Eigen::VectorXf EstimatorContinuousDiscrete::attitude_dynamics(const Eigen::Vect return f; } -Eigen::VectorXf EstimatorContinuousDiscrete::position_dynamics(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements) +Eigen::VectorXf EstimatorContinuousDiscrete::position_dynamics(const Eigen::VectorXf & state, + const Eigen::VectorXf & measurements) { double gravity = params_.get_double("gravity"); @@ -382,7 +396,7 @@ Eigen::VectorXf EstimatorContinuousDiscrete::position_dynamics(const Eigen::Vect float psidot = (q * sinf(phi) + r * cosf(phi)) / cosf(theta); float Vgdot = va / Vg * psidot * (we * cosf(psi) - wn * sinf(psi)); - + Eigen::VectorXf f; f = Eigen::VectorXf::Zero(7); @@ -395,13 +409,15 @@ Eigen::VectorXf EstimatorContinuousDiscrete::position_dynamics(const Eigen::Vect return f; } -Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& angular_rates) +Eigen::MatrixXf +EstimatorContinuousDiscrete::attitude_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & angular_rates) { float cp = cosf(state(0)); // cos(phi) float sp = sinf(state(0)); // sin(phi) float tt = tanf(state(1)); // tan(theta) float ct = cosf(state(1)); // cos(theta) - + float q = angular_rates(1); float r = angular_rates(2); @@ -413,7 +429,8 @@ Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_jacobian(const Eigen::Vect return A; } -Eigen::MatrixXf EstimatorContinuousDiscrete::position_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& measurements) +Eigen::MatrixXf EstimatorContinuousDiscrete::position_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & measurements) { double gravity = params_.get_double("gravity"); @@ -423,15 +440,15 @@ Eigen::MatrixXf EstimatorContinuousDiscrete::position_jacobian(const Eigen::Vect float phi = measurements(3); float theta = measurements(4); float va = measurements(5); - + float Vg = state(2); float chi = state(3); float wn = state(4); float we = state(5); float psi = state(6); - + float psidot = (q * sinf(phi) + r * cosf(phi)) / cosf(theta); - + float tmp = -psidot * va * (state(4) * cosf(state(6)) + state(5) * sinf(state(6))) / state(2); float Vgdot = va / Vg * psidot * (wn * cosf(psi) - we * sinf(psi)); @@ -451,7 +468,8 @@ Eigen::MatrixXf EstimatorContinuousDiscrete::position_jacobian(const Eigen::Vect return A; } -Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_input_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & inputs) { float cp = cosf(state(0)); // cos(phi) float sp = sinf(state(0)); // sin(phi) @@ -463,23 +481,26 @@ Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_input_jacobian(const Eigen return G; } -Eigen::MatrixXf EstimatorContinuousDiscrete::position_input_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +Eigen::MatrixXf EstimatorContinuousDiscrete::position_input_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & inputs) { - + Eigen::MatrixXf G; G = Eigen::MatrixXf::Zero(7, 7); return G; } -Eigen::VectorXf EstimatorContinuousDiscrete::attitude_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +Eigen::VectorXf +EstimatorContinuousDiscrete::attitude_measurement_prediction(const Eigen::VectorXf & state, + const Eigen::VectorXf & inputs) { double gravity = params_.get_double("gravity"); float cp = cosf(state(0)); // cos(phi) float sp = sinf(state(0)); // sin(phi) float st = sinf(state(1)); // sin(theta) float ct = cosf(state(1)); // cos(theta) - + float p = inputs(0); float q = inputs(1); float r = inputs(2); @@ -494,10 +515,12 @@ Eigen::VectorXf EstimatorContinuousDiscrete::attitude_measurement_prediction(con return h; } -Eigen::VectorXf EstimatorContinuousDiscrete::position_measurement_prediction(const Eigen::VectorXf& state, const Eigen::VectorXf& input) +Eigen::VectorXf +EstimatorContinuousDiscrete::position_measurement_prediction(const Eigen::VectorXf & state, + const Eigen::VectorXf & input) { float va = input(0); - + Eigen::VectorXf h = Eigen::VectorXf::Zero(6); // GPS north @@ -508,44 +531,46 @@ Eigen::VectorXf EstimatorContinuousDiscrete::position_measurement_prediction(con // GPS ground speed h(2) = state(2); - + // GPS course h(3) = state(3); - + // Pseudo Measurement north h(4) = va * cosf(state(6)) + state(4) - state(2) * cosf(state(3)); - + // Pseudo Measurement east h(5) = va * sinf(state(6)) + state(5) - state(2) * sinf(state(3)); // To add a new measurement, simply use the state and any input you need as another entry to h. Be sure to update the measurement jacobian C. - + return h; } -Eigen::MatrixXf EstimatorContinuousDiscrete::position_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& input) +Eigen::MatrixXf +EstimatorContinuousDiscrete::position_measurement_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & input) { float va = input(0); - Eigen::MatrixXf C = Eigen::MatrixXf::Zero(6,7); - + Eigen::MatrixXf C = Eigen::MatrixXf::Zero(6, 7); + // GPS north - C(0,0) = 1; + C(0, 0) = 1; // GPS east - C(1,1) = 1; - + C(1, 1) = 1; + // GPS ground speed - C(2,2) = 1; + C(2, 2) = 1; // GPS course - C(3,3) = 1; - + C(3, 3) = 1; + // Pseudo Measurement north - C(4,2) = -cos(state(3)); - C(4,3) = state(2) * sinf(state(3)); - C(4,4) = 1; - C(4,6) = -va * sinf(state(6)); + C(4, 2) = -cos(state(3)); + C(4, 3) = state(2) * sinf(state(3)); + C(4, 4) = 1; + C(4, 6) = -va * sinf(state(6)); // Pseudo Measurement east C(5, 2) = -sin(state(3)); @@ -558,14 +583,16 @@ Eigen::MatrixXf EstimatorContinuousDiscrete::position_measurement_jacobian(const return C; } -Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_measurement_jacobian(const Eigen::VectorXf& state, const Eigen::VectorXf& inputs) +Eigen::MatrixXf +EstimatorContinuousDiscrete::attitude_measurement_jacobian(const Eigen::VectorXf & state, + const Eigen::VectorXf & inputs) { double gravity = params_.get_double("gravity"); float cp = cosf(state(0)); float sp = sinf(state(0)); float ct = cosf(state(1)); float st = sinf(state(1)); - + float p = inputs(0); float q = inputs(1); float r = inputs(2); @@ -574,8 +601,7 @@ Eigen::MatrixXf EstimatorContinuousDiscrete::attitude_measurement_jacobian(const Eigen::Matrix C; C << 0.0, q * va * ct + gravity * ct, -gravity * cp * ct, - -r * va * st - p * va * ct + gravity * sp * st, gravity * sp * ct, - (q * va + gravity * cp) * st; + -r * va * st - p * va * ct + gravity * sp * st, gravity * sp * ct, (q * va + gravity * cp) * st; return C; } @@ -588,18 +614,18 @@ void EstimatorContinuousDiscrete::check_xhat_a() if (xhat_a_(0) > radians(85.0) || xhat_a_(0) < radians(-85.0) || !std::isfinite(xhat_a_(0))) { - if (!std::isfinite(xhat_a_(0))) { - xhat_a_(0) = 0; - P_a_ = Eigen::Matrix2f::Identity(); - P_a_ *= powf(radians(20.0f), 2); - RCLCPP_WARN(this->get_logger(), "attiude estimator reinitialized due to non-finite roll"); - } else if (xhat_a_(0) > radians(max_phi)) { - xhat_a_(0) = radians(max_phi - buff); - RCLCPP_WARN(this->get_logger(), "max roll angle"); - } else if (xhat_a_(0) < radians(-max_phi)) { - xhat_a_(0) = radians(-max_phi + buff); - RCLCPP_WARN(this->get_logger(), "min roll angle"); - } + if (!std::isfinite(xhat_a_(0))) { + xhat_a_(0) = 0; + P_a_ = Eigen::Matrix2f::Identity(); + P_a_ *= powf(radians(20.0f), 2); + RCLCPP_WARN(this->get_logger(), "attiude estimator reinitialized due to non-finite roll"); + } else if (xhat_a_(0) > radians(max_phi)) { + xhat_a_(0) = radians(max_phi - buff); + RCLCPP_WARN(this->get_logger(), "max roll angle"); + } else if (xhat_a_(0) < radians(-max_phi)) { + xhat_a_(0) = radians(-max_phi + buff); + RCLCPP_WARN(this->get_logger(), "min roll angle"); + } } if (!std::isfinite(xhat_a_(1))) { @@ -631,25 +657,25 @@ void EstimatorContinuousDiscrete::declare_parameters() params_.declare_double("gps_n_lim", 10000.); params_.declare_double("gps_e_lim", 10000.); - params_.declare_double("roll_process_noise", 0.0001); // Radians?, should be already squared - params_.declare_double("pitch_process_noise", 0.0000001); // Radians?, already squared - params_.declare_double("gyro_process_noise", 0.13); // Deg, not squared - params_.declare_double("pos_process_noise", 0.1); // already squared + params_.declare_double("roll_process_noise", 0.0001); // Radians?, should be already squared + params_.declare_double("pitch_process_noise", 0.0000001); // Radians?, already squared + params_.declare_double("gyro_process_noise", 0.13); // Deg, not squared + params_.declare_double("pos_process_noise", 0.1); // already squared params_.declare_double("attitude_initial_cov", 5.0); // Deg, not squared params_.declare_double("pos_n_initial_cov", 0.03); params_.declare_double("pos_e_initial_cov", 0.03); params_.declare_double("vg_initial_cov", 0.01); - params_.declare_double("chi_initial_cov", 5.0); // Deg + params_.declare_double("chi_initial_cov", 5.0); // Deg params_.declare_double("wind_n_initial_cov", 0.04); params_.declare_double("wind_e_initial_cov", 0.04); - params_.declare_double("psi_initial_cov", 5.0); // Deg + params_.declare_double("psi_initial_cov", 5.0); // Deg params_.declare_int("num_propagation_steps", 10); - params_.declare_double("max_estimated_phi", 85.0); // Deg + params_.declare_double("max_estimated_phi", 85.0); // Deg params_.declare_double("max_estimated_theta", 80.0); // Deg - params_.declare_double("estimator_max_buffer", 3.0); // Deg + params_.declare_double("estimator_max_buffer", 3.0); // Deg } void EstimatorContinuousDiscrete::bind_functions() @@ -657,17 +683,30 @@ void EstimatorContinuousDiscrete::bind_functions() // This creates references to the functions that are necessary estimate. This means we can pass them to the EKF class's functions. // std::bind creates a forwarding reference to a function. So when we pass the binding object to another method, that method can call the // original function. - attitude_dynamics_model = std::bind(&EstimatorContinuousDiscrete::attitude_dynamics, this, std::placeholders::_1, std::placeholders::_2); - attitude_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_jacobian, this, std::placeholders::_1, std::placeholders::_2); - attitude_input_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_input_jacobian, this, std::placeholders::_1, std::placeholders::_2); - attitude_measurement_model = std::bind(&EstimatorContinuousDiscrete::attitude_measurement_prediction, this, std::placeholders::_1, std::placeholders::_2); - attitude_measurement_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_measurement_jacobian, this, std::placeholders::_1, std::placeholders::_2); - position_dynamics_model = std::bind(&EstimatorContinuousDiscrete::position_dynamics, this, std::placeholders::_1, std::placeholders::_2); - position_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_jacobian, this, std::placeholders::_1, std::placeholders::_2); - position_input_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_input_jacobian, this, std::placeholders::_1, std::placeholders::_2); - position_measurement_model = std::bind(&EstimatorContinuousDiscrete::position_measurement_prediction, this, std::placeholders::_1, std::placeholders::_2); - position_measurement_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_measurement_jacobian, this, std::placeholders::_1, std::placeholders::_2); - + attitude_dynamics_model = std::bind(&EstimatorContinuousDiscrete::attitude_dynamics, this, + std::placeholders::_1, std::placeholders::_2); + attitude_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_jacobian, this, + std::placeholders::_1, std::placeholders::_2); + attitude_input_jacobian_model = std::bind(&EstimatorContinuousDiscrete::attitude_input_jacobian, + this, std::placeholders::_1, std::placeholders::_2); + attitude_measurement_model = + std::bind(&EstimatorContinuousDiscrete::attitude_measurement_prediction, this, + std::placeholders::_1, std::placeholders::_2); + attitude_measurement_jacobian_model = + std::bind(&EstimatorContinuousDiscrete::attitude_measurement_jacobian, this, + std::placeholders::_1, std::placeholders::_2); + position_dynamics_model = std::bind(&EstimatorContinuousDiscrete::position_dynamics, this, + std::placeholders::_1, std::placeholders::_2); + position_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_jacobian, this, + std::placeholders::_1, std::placeholders::_2); + position_input_jacobian_model = std::bind(&EstimatorContinuousDiscrete::position_input_jacobian, + this, std::placeholders::_1, std::placeholders::_2); + position_measurement_model = + std::bind(&EstimatorContinuousDiscrete::position_measurement_prediction, this, + std::placeholders::_1, std::placeholders::_2); + position_measurement_jacobian_model = + std::bind(&EstimatorContinuousDiscrete::position_measurement_jacobian, this, + std::placeholders::_1, std::placeholders::_2); } } // namespace rosplane diff --git a/rosplane/src/estimator_ekf.cpp b/rosplane/src/estimator_ekf.cpp index 01b7862..36dc0eb 100644 --- a/rosplane/src/estimator_ekf.cpp +++ b/rosplane/src/estimator_ekf.cpp @@ -9,86 +9,86 @@ namespace rosplane { -EstimatorEKF::EstimatorEKF() : EstimatorROS() +EstimatorEKF::EstimatorEKF() + : EstimatorROS() {} - -std::tuple EstimatorEKF::measurement_update(Eigen::VectorXf x, - Eigen::VectorXf inputs, - std::function measurement_model, - Eigen::VectorXf y, - std::function measurement_jacobian, - Eigen::MatrixXf R, - Eigen::MatrixXf P) + +std::tuple EstimatorEKF::measurement_update( + Eigen::VectorXf x, Eigen::VectorXf inputs, + std::function measurement_model, + Eigen::VectorXf y, + std::function measurement_jacobian, + Eigen::MatrixXf R, Eigen::MatrixXf P) { - + Eigen::VectorXf h = measurement_model(x, inputs); Eigen::MatrixXf C = measurement_jacobian(x, inputs); - + // Find the S_inv to find the Kalman gain. Eigen::MatrixXf S_inv = (R + C * P * C.transpose()).inverse(); // Find the Kalman gain. Eigen::MatrixXf L = P * C.transpose() * S_inv; // Use a temp to increase readablility. Eigen::MatrixXf temp = Eigen::MatrixXf::Identity(x.size(), x.size()) - L * C; - + // Adjust the covariance with new information. P = temp * P * temp.transpose() + L * R * L.transpose(); // Use Kalman gain to optimally adjust estimate. x = x + L * (y - h); std::tuple result(P, x); - + return result; } -std::tuple EstimatorEKF::propagate_model(Eigen::VectorXf x, - std::function dynamic_model, - std::function jacobian, - Eigen::VectorXf inputs, - std::function input_jacobian, - Eigen::MatrixXf P, - Eigen::MatrixXf Q, - Eigen::MatrixXf Q_g, - float Ts) +std::tuple EstimatorEKF::propagate_model( + Eigen::VectorXf x, + std::function dynamic_model, + std::function jacobian, + Eigen::VectorXf inputs, + std::function input_jacobian, + Eigen::MatrixXf P, Eigen::MatrixXf Q, Eigen::MatrixXf Q_g, float Ts) { - int N = params_.get_int("num_propagation_steps"); // TODO: Declare this parameter in the ekf class. + int N = + params_.get_int("num_propagation_steps"); // TODO: Declare this parameter in the ekf class. - for (int _ = 0; _ < N; _++) - { + for (int _ = 0; _ < N; _++) { Eigen::VectorXf f = dynamic_model(x, inputs); // Propagate model by a step. - x += f * (Ts/N); + x += f * (Ts / N); Eigen::MatrixXf A = jacobian(x, inputs); - + // Find the second order approx of the matrix exponential. - Eigen::MatrixXf A_d = Eigen::MatrixXf::Identity(A.rows(), A.cols()) + Ts / N * A - + pow(Ts / N, 2) / 2.0 * A * A; + Eigen::MatrixXf A_d = + Eigen::MatrixXf::Identity(A.rows(), A.cols()) + Ts / N * A + pow(Ts / N, 2) / 2.0 * A * A; Eigen::MatrixXf G = input_jacobian(x, inputs); // Propagate the covariance. P = A_d * P * A_d.transpose() + (Q + G * Q_g * G.transpose()) * pow(Ts / N, 2); - } std::tuple result(P, x); - + return result; } -std::tuple EstimatorEKF::single_measurement_update(float measurement, float measurement_prediction, float measurement_variance, Eigen::VectorXf measurement_jacobian, Eigen::VectorXf x, Eigen::MatrixXf P) +std::tuple EstimatorEKF::single_measurement_update( + float measurement, float measurement_prediction, float measurement_variance, + Eigen::VectorXf measurement_jacobian, Eigen::VectorXf x, Eigen::MatrixXf P) { - Eigen::MatrixXf I(x.size(),x.size()); + Eigen::MatrixXf I(x.size(), x.size()); I = Eigen::MatrixXf::Identity(x.size(), x.size()); - Eigen::VectorXf L = (P * measurement_jacobian) / (measurement_variance + (measurement_jacobian.transpose() * P * measurement_jacobian)); + Eigen::VectorXf L = (P * measurement_jacobian) + / (measurement_variance + (measurement_jacobian.transpose() * P * measurement_jacobian)); P = (I - L * measurement_jacobian.transpose()) * P; x = x + L * (measurement - measurement_prediction); - std::tuple result(P,x); + std::tuple result(P, x); return result; } -} // end nampspace. +} // namespace rosplane diff --git a/rosplane/src/estimator_ros.cpp b/rosplane/src/estimator_ros.cpp index 1234a13..6e6ecc3 100644 --- a/rosplane/src/estimator_ros.cpp +++ b/rosplane/src/estimator_ros.cpp @@ -3,14 +3,16 @@ #include #include -#include "estimator_ros.hpp" #include "estimator_continuous_discrete.hpp" +#include "estimator_ros.hpp" namespace rosplane { EstimatorROS::EstimatorROS() - : Node("estimator_ros"), params_(this), params_initialized_(false) + : Node("estimator_ros") + , params_(this) + , params_initialized_(false) { vehicle_state_pub_ = this->create_publisher("estimated_state", 10); @@ -47,7 +49,7 @@ EstimatorROS::EstimatorROS() std::filesystem::path params_dir = "params"; std::filesystem::path params_file = "anaconda_autopilot_params.yaml"; - std::filesystem::path full_path = rosplane_dir/params_dir/params_file; + std::filesystem::path full_path = rosplane_dir / params_dir / params_file; param_filepath_ = full_path.string(); @@ -59,24 +61,29 @@ void EstimatorROS::declare_parameters() params_.declare_double("estimator_update_frequency", 100.0); params_.declare_double("rho", 1.225); params_.declare_double("gravity", 9.8); - params_.declare_double("gps_ground_speed_threshold", 0.3); // TODO: this is a magic number. What is it determined from? - params_.declare_double("baro_measurement_gate", 1.35); // TODO: this is a magic number. What is it determined from? - params_.declare_double("airspeed_measurement_gate", 5.0); // TODO: this is a magic number. What is it determined from? - params_.declare_int("baro_calibration_count", 100); // TODO: this is a magic number. What is it determined from? + params_.declare_double("gps_ground_speed_threshold", + 0.3); // TODO: this is a magic number. What is it determined from? + params_.declare_double("baro_measurement_gate", + 1.35); // TODO: this is a magic number. What is it determined from? + params_.declare_double("airspeed_measurement_gate", + 5.0); // TODO: this is a magic number. What is it determined from? + params_.declare_int("baro_calibration_count", + 100); // TODO: this is a magic number. What is it determined from? params_.declare_double("baro_calibration_val", 0.0); params_.declare_double("init_lat", 0.0); params_.declare_double("init_lon", 0.0); params_.declare_double("init_alt", 0.0); } -void EstimatorROS::set_timer() { +void EstimatorROS::set_timer() +{ double frequency = params_.get_double("estimator_update_frequency"); update_period_ = std::chrono::microseconds(static_cast(1.0 / frequency * 1'000'000)); update_timer_ = this->create_wall_timer(update_period_, std::bind(&EstimatorROS::update, this)); } -rcl_interfaces::msg::SetParametersResult +rcl_interfaces::msg::SetParametersResult EstimatorROS::parametersCallback(const std::vector & parameters) { rcl_interfaces::msg::SetParametersResult result; @@ -84,21 +91,19 @@ EstimatorROS::parametersCallback(const std::vector & paramete result.reason = "success"; bool success = params_.set_parameters_callback(parameters); - if (!success) - { + if (!success) { result.successful = false; - result.reason = - "One of the parameters given is not a parameter of the estimator node."; + result.reason = "One of the parameters given is not a parameter of the estimator node."; } // Check to see if the timer period was changed. If it was, recreate the timer with the new period if (params_initialized_ && success) { - std::chrono::microseconds curr_period = std::chrono::microseconds(static_cast(1.0 / params_.get_double("estimator_update_frequency") * 1'000'000)); + std::chrono::microseconds curr_period = std::chrono::microseconds( + static_cast(1.0 / params_.get_double("estimator_update_frequency") * 1'000'000)); if (update_period_ != curr_period) { update_timer_->cancel(); set_timer(); } - } return result; @@ -122,8 +127,7 @@ void EstimatorROS::update() rosplane_msgs::msg::State msg; msg.header.stamp = this->get_clock()->now(); - msg.header.frame_id = - 1; // Denotes global frame + msg.header.frame_id = 1; // Denotes global frame msg.position[0] = output.pn; msg.position[1] = output.pe; @@ -299,7 +303,8 @@ void EstimatorROS::airspeedCallback(const rosflight_msgs::msg::Airspeed::SharedP void EstimatorROS::statusCallback(const rosflight_msgs::msg::Status::SharedPtr msg) { - if (!armed_first_time_ && msg->armed) armed_first_time_ = true; + if (!armed_first_time_ && msg->armed) + armed_first_time_ = true; } void EstimatorROS::saveParameter(std::string param_name, double param_val) @@ -307,13 +312,11 @@ void EstimatorROS::saveParameter(std::string param_name, double param_val) YAML::Node param_yaml_file = YAML::LoadFile(param_filepath_); - if (param_yaml_file["estimator"]["ros__parameters"][param_name]) - { + if (param_yaml_file["estimator"]["ros__parameters"][param_name]) { param_yaml_file["estimator"]["ros__parameters"][param_name] = param_val; - } - else - { - RCLCPP_ERROR_STREAM(this->get_logger(), "Parameter [" << param_name << "] is not in parameter file."); + } else { + RCLCPP_ERROR_STREAM(this->get_logger(), + "Parameter [" << param_name << "] is not in parameter file."); } std::ofstream fout(param_filepath_); @@ -324,30 +327,25 @@ void EstimatorROS::saveParameter(std::string param_name, double param_val) int main(int argc, char ** argv) { - + rclcpp::init(argc, argv); - char* use_params; // HACK: Fix in a more permanant way. + char * use_params; // HACK: Fix in a more permanant way. if (argc >= 1) { use_params = argv[1]; } - - if (!strcmp(use_params, "true")) - { + if (!strcmp(use_params, "true")) { rclcpp::spin(std::make_shared(use_params)); - } - else if(strcmp(use_params, "false")) // If the string is not true or false print error. + } else if (strcmp(use_params, "false")) // If the string is not true or false print error. { auto estimator_node = std::make_shared(); - RCLCPP_WARN(estimator_node->get_logger(), "Invalid option for seeding estimator, defaulting to unseeded."); + RCLCPP_WARN(estimator_node->get_logger(), + "Invalid option for seeding estimator, defaulting to unseeded."); rclcpp::spin(estimator_node); - } - else - { + } else { rclcpp::spin(std::make_shared()); } - return 0; } diff --git a/rosplane/src/param_manager/param_manager.cpp b/rosplane/src/param_manager/param_manager.cpp index cf87095..42fa7e0 100644 --- a/rosplane/src/param_manager/param_manager.cpp +++ b/rosplane/src/param_manager/param_manager.cpp @@ -5,7 +5,9 @@ namespace rosplane { -ParamManager::ParamManager(rclcpp::Node * node) : container_node_{node} {} +ParamManager::ParamManager(rclcpp::Node * node) + : container_node_{node} +{} void ParamManager::declare_double(std::string param_name, double value) { @@ -42,9 +44,9 @@ void ParamManager::declare_string(std::string param_name, std::string value) void ParamManager::set_double(std::string param_name, double value) { // Check that the parameter is in the parameter struct - if (params_.find(param_name) == params_.end()) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name); + if (params_.find(param_name) == params_.end()) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), + "Parameter not found in parameter struct: " + param_name); return; } @@ -57,9 +59,9 @@ void ParamManager::set_double(std::string param_name, double value) void ParamManager::set_bool(std::string param_name, bool value) { // Check that the parameter is in the parameter struct - if (params_.find(param_name) == params_.end()) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name); + if (params_.find(param_name) == params_.end()) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), + "Parameter not found in parameter struct: " + param_name); return; } @@ -72,9 +74,9 @@ void ParamManager::set_bool(std::string param_name, bool value) void ParamManager::set_int(std::string param_name, int64_t value) { // Check that the parameter is in the parameter struct - if (params_.find(param_name) == params_.end()) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name); + if (params_.find(param_name) == params_.end()) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), + "Parameter not found in parameter struct: " + param_name); return; } @@ -87,9 +89,9 @@ void ParamManager::set_int(std::string param_name, int64_t value) void ParamManager::set_string(std::string param_name, std::string value) { // Check that the parameter is in the parameter struct - if (params_.find(param_name) == params_.end()) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Parameter not found in parameter struct: " + param_name); + if (params_.find(param_name) == params_.end()) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), + "Parameter not found in parameter struct: " + param_name); return; } @@ -101,63 +103,50 @@ void ParamManager::set_string(std::string param_name, std::string value) double ParamManager::get_double(std::string param_name) { - try - { - return std::get(params_[param_name]); - } - catch (std::bad_variant_access & e) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); - throw std::runtime_error(e.what()); - } -} + try { + return std::get(params_[param_name]); + } catch (std::bad_variant_access & e) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); + throw std::runtime_error(e.what()); + } +} bool ParamManager::get_bool(std::string param_name) { - try - { - return std::get(params_[param_name]); - } - catch (std::bad_variant_access & e) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); - throw std::runtime_error(e.what()); - } -} + try { + return std::get(params_[param_name]); + } catch (std::bad_variant_access & e) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); + throw std::runtime_error(e.what()); + } +} int64_t ParamManager::get_int(std::string param_name) { - try - { - return std::get(params_[param_name]); - } - catch (std::bad_variant_access & e) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); - throw std::runtime_error(e.what()); - } -} + try { + return std::get(params_[param_name]); + } catch (std::bad_variant_access & e) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); + throw std::runtime_error(e.what()); + } +} std::string ParamManager::get_string(std::string param_name) { - try - { - return std::get(params_[param_name]); - } - catch (std::bad_variant_access & e) - { - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); - throw std::runtime_error(e.what()); - } -} + try { + return std::get(params_[param_name]); + } catch (std::bad_variant_access & e) { + RCLCPP_ERROR_STREAM(container_node_->get_logger(), "ERROR GETTING PARAMETER: " + param_name); + throw std::runtime_error(e.what()); + } +} void ParamManager::set_parameters() { // Get the parameters from the launch file, if given. // If not, use the default value defined at declaration - for (const auto& [key, value] : params_) - { + for (const auto & [key, value] : params_) { auto type = container_node_->get_parameter(key).get_type(); if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) params_[key] = container_node_->get_parameter(key).as_double(); @@ -167,8 +156,10 @@ void ParamManager::set_parameters() params_[key] = container_node_->get_parameter(key).as_int(); else if (type == rclcpp::ParameterType::PARAMETER_STRING) params_[key] = container_node_->get_parameter(key).as_string(); - else - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Unable to set parameter: " + key + ". Error casting parameter as double, int, string, or bool!"); + else + RCLCPP_ERROR_STREAM(container_node_->get_logger(), + "Unable to set parameter: " + key + + ". Error casting parameter as double, int, string, or bool!"); } } @@ -176,10 +167,13 @@ bool ParamManager::set_parameters_callback(const std::vector { // Check each parameter in the incoming vector of parameters to change, and change the appropriate parameter. for (const auto & param : parameters) { - + // 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 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; } @@ -192,10 +186,11 @@ bool ParamManager::set_parameters_callback(const std::vector else if (param.get_type() == rclcpp::ParameterType::PARAMETER_STRING) params_[param.get_name()] = param.as_string(); else - RCLCPP_ERROR_STREAM(container_node_->get_logger(), "Unable to determine parameter type in controller. Type is " + std::to_string(param.get_type())); - + RCLCPP_ERROR_STREAM(container_node_->get_logger(), + "Unable to determine parameter type in controller. Type is " + + std::to_string(param.get_type())); } return true; } -} // namespace rosplane \ No newline at end of file +} // namespace rosplane \ No newline at end of file diff --git a/rosplane/src/path_follower_base.cpp b/rosplane/src/path_follower_base.cpp index 8d02b8a..0f1f550 100644 --- a/rosplane/src/path_follower_base.cpp +++ b/rosplane/src/path_follower_base.cpp @@ -8,14 +8,15 @@ namespace rosplane { PathFollowerBase::PathFollowerBase() - : Node("path_follower_base"), params_(this), params_initialized_(false) + : Node("path_follower_base") + , params_(this) + , params_initialized_(false) { vehicle_state_sub_ = this->create_subscription( "estimated_state", 10, std::bind(&PathFollowerBase::vehicle_state_callback, this, _1)); current_path_sub_ = this->create_subscription( - "current_path", 100, - std::bind(&PathFollowerBase::current_path_callback, this, _1)); + "current_path", 100, std::bind(&PathFollowerBase::current_path_callback, this, _1)); controller_commands_pub_ = this->create_publisher("controller_command", 1); @@ -37,15 +38,14 @@ PathFollowerBase::PathFollowerBase() current_path_init_ = false; } -void PathFollowerBase::set_timer() { +void PathFollowerBase::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)); - update_timer_ = this->create_wall_timer( - timer_period_, - std::bind(&PathFollowerBase::update, - this)); + update_timer_ = + this->create_wall_timer(timer_period_, std::bind(&PathFollowerBase::update, this)); } void PathFollowerBase::update() @@ -88,8 +88,7 @@ void PathFollowerBase::current_path_callback(const rosplane_msgs::msg::CurrentPa { if (msg->path_type == msg->LINE_PATH) { input_.p_type = PathType::LINE; - } - else if (msg->path_type == msg->ORBIT_PATH) { + } else if (msg->path_type == msg->ORBIT_PATH) { input_.p_type = PathType::ORBIT; } @@ -114,8 +113,7 @@ PathFollowerBase::parametersCallback(const std::vector & para // Update the param_manager object with the new parameters bool success = params_.set_parameters_callback(parameters); - if (success) - { + if (success) { result.successful = true; result.reason = "success"; } @@ -124,7 +122,8 @@ PathFollowerBase::parametersCallback(const std::vector & para if (params_initialized_ && success) { double frequency = params_.get_double("controller_commands_pub_frequency"); - std::chrono::microseconds curr_period = std::chrono::microseconds(static_cast(1.0 / frequency * 1e6)); + std::chrono::microseconds curr_period = + std::chrono::microseconds(static_cast(1.0 / frequency * 1e6)); if (timer_period_ != curr_period) { update_timer_->cancel(); set_timer(); diff --git a/rosplane/src/path_follower_example.cpp b/rosplane/src/path_follower_example.cpp index 9550222..ec2478d 100644 --- a/rosplane/src/path_follower_example.cpp +++ b/rosplane/src/path_follower_example.cpp @@ -12,8 +12,7 @@ double wrap_within_180(double fixed_heading, double wrapped_heading) PathFollowerExample::PathFollowerExample() {} -void PathFollowerExample::follow(const Input & input, - Output & output) +void PathFollowerExample::follow(const Input & input, Output & output) { // For readability, declare parameters that will be used in the function here double k_path = params_.get_double("k_path"); @@ -46,8 +45,7 @@ void PathFollowerExample::follow(const Input & input, // commanded altitude is desired altitude output.h_c = h_d; output.phi_ff = 0.0; - } - else { + } else { float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + powf((input.pe - input.c_orbit[1]), 2)); // distance from orbit center @@ -59,8 +57,7 @@ void PathFollowerExample::follow(const Input & input, //compute orbit error float norm_orbit_error = (d - input.rho_orbit) / input.rho_orbit; - output.chi_c = - varphi + input.lam_orbit * (M_PI / 2.0 + atanf(k_orbit * norm_orbit_error)); + output.chi_c = varphi + input.lam_orbit * (M_PI / 2.0 + atanf(k_orbit * norm_orbit_error)); // commanded altitude is the height of the orbit float h_d = -input.c_orbit[2]; diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp index 7788292..3b61fb4 100644 --- a/rosplane/src/path_manager_base.cpp +++ b/rosplane/src/path_manager_base.cpp @@ -12,7 +12,9 @@ namespace rosplane { PathManagerBase::PathManagerBase() - : 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(&PathManagerBase::vehicle_state_callback, this, _1)); @@ -38,14 +40,16 @@ PathManagerBase::PathManagerBase() state_init_ = false; } -void PathManagerBase::declare_parameters() { +void PathManagerBase::declare_parameters() +{ params_.declare_double("R_min", 50.0); params_.declare_double("current_path_pub_frequency", 100.0); params_.declare_double("default_altitude", 50.0); params_.declare_double("default_airspeed", 15.0); } -void PathManagerBase::set_timer() { +void PathManagerBase::set_timer() +{ // 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)); @@ -63,8 +67,7 @@ PathManagerBase::parametersCallback(const std::vector & param // Update the changed parameters in the param_manager object bool success = params_.set_parameters_callback(parameters); - if (success) - { + if (success) { result.successful = true; result.reason = "success"; } @@ -72,7 +75,8 @@ PathManagerBase::parametersCallback(const std::vector & param // If the frequency parameter was changed, restart the timer. if (params_initialized_ && success) { double frequency = params_.get_double("current_path_pub_frequency"); - std::chrono::microseconds curr_period = std::chrono::microseconds(static_cast(1.0 / frequency * 1e6)); + std::chrono::microseconds curr_period = + std::chrono::microseconds(static_cast(1.0 / frequency * 1e6)); if (timer_period_ != curr_period) { update_timer_->cancel(); set_timer(); @@ -106,18 +110,16 @@ void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & // 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) - { + if (waypoints_.size() == 0) { Waypoint temp_waypoint; temp_waypoint.w[0] = vehicle_state_.position[0]; temp_waypoint.w[1] = vehicle_state_.position[1]; - + if (vehicle_state_.position[2] < -default_altitude) { - + temp_waypoint.w[2] = vehicle_state_.position[2]; - } - else { + } else { temp_waypoint.w[2] = -default_altitude; } @@ -129,7 +131,7 @@ void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & num_waypoints_++; temp_waypoint_ = true; } - + // Add a default comparison for the last waypoint for feasiblity check. Waypoint nextwp; Eigen::Vector3f w_existing(std::numeric_limits::infinity(), @@ -143,8 +145,7 @@ void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & nextwp.va_d = msg.va_d; // Save the last waypoint for comparison. - if (waypoints_.size() > 0) - { + if (waypoints_.size() > 0) { Waypoint waypoint = waypoints_.back(); w_existing << waypoint.w[0], waypoint.w[1], waypoint.w[2]; } @@ -154,9 +155,10 @@ void PathManagerBase::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & // Warn if too close to the last waypoint. Eigen::Vector3f w_new(msg.w[0], msg.w[1], msg.w[2]); - if ((w_new - w_existing).norm() < R_min) - { - RCLCPP_WARN_STREAM(this->get_logger(), "A waypoint is too close to the next waypoint. Indices: " << waypoints_.size() - 2 << ", " << waypoints_.size() - 1); + if ((w_new - w_existing).norm() < R_min) { + RCLCPP_WARN_STREAM(this->get_logger(), + "A waypoint is too close to the next waypoint. Indices: " + << waypoints_.size() - 2 << ", " << waypoints_.size() - 1); } } @@ -183,8 +185,7 @@ void PathManagerBase::current_path_publish() current_path.header.stamp = now; if (output.flag) { current_path.path_type = current_path.LINE_PATH; - } - else { + } else { current_path.path_type = current_path.ORBIT_PATH; } current_path.va_d = output.va_d; diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp index d8a3a5b..aea08a8 100644 --- a/rosplane/src/path_manager_example.cpp +++ b/rosplane/src/path_manager_example.cpp @@ -26,33 +26,35 @@ void PathManagerExample::manage(const Input & input, Output & 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_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) - { + if (num_waypoints_ == 0) { auto now = std::chrono::system_clock::now(); - if (float(std::chrono::system_clock::to_time_t(now) - std::chrono::system_clock::to_time_t(start_time_)) >= 10.0) - { + if (float(std::chrono::system_clock::to_time_t(now) + - std::chrono::system_clock::to_time_t(start_time_)) + >= 10.0) { // TODO: Add check to see if the aircraft has been armed. If not just send the warning once before flight then on the throttle after. - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "No waypoints received, orbiting origin at " << default_altitude << " meters."); - output.flag = false; // Indicate that the path is an orbit. + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "No waypoints received, orbiting origin at " << default_altitude + << " meters."); + output.flag = false; // Indicate that the path is an orbit. output.va_d = default_airspeed; // Set to the default_airspeed. - output.q[0] = 0.0f; // initialize the parameters to have a value. + output.q[0] = 0.0f; // initialize the parameters to have a value. output.q[1] = 0.0f; output.q[2] = 0.0f; output.r[0] = 0.0f; // initialize the parameters to have a value. output.r[1] = 0.0f; output.r[2] = 0.0f; - output.c[0] = 0.0f; // Direcct the center of the orbit to the origin at the default default_altitude. + output.c[0] = + 0.0f; // Direcct the center of the orbit to the origin at the default default_altitude. output.c[1] = 0.0f; output.c[2] = -default_altitude; output.rho = R_min; // Make the orbit at the minimum turn radius. - output.lamda = 1; // Orbit in a clockwise manner. + output.lamda = 1; // Orbit in a clockwise manner. } - } - else if (num_waypoints_ == 1) - { + } else if (num_waypoints_ == 1) { // If only a single waypoint is given, orbit it. output.flag = false; output.va_d = waypoints_[0].va_d; @@ -66,9 +68,10 @@ void PathManagerExample::manage(const Input & input, Output & output) output.c[1] = waypoints_[0].w[1]; output.c[2] = waypoints_[0].w[2]; output.rho = R_min; - output.lamda = orbit_direction(input.pn, input.pe, input.chi, output.c[0], output.c[1]); // Calculate the most conveinent orbit direction of that point. - } - else { + output.lamda = + orbit_direction(input.pn, input.pe, input.chi, output.c[0], + output.c[1]); // Calculate the most conveinent orbit direction of that point. + } else { if (waypoints_[idx_a_].use_chi) { manage_dubins(input, output); } else { // If the heading through the point does not matter use the default path following. @@ -79,8 +82,7 @@ void PathManagerExample::manage(const Input & input, Output & output) } } -void PathManagerExample::manage_line(const Input & input, - Output & output) +void PathManagerExample::manage_line(const Input & input, Output & output) { // For readability, declare the parameters that will be used in the function here bool orbit_last = params_.get_bool("orbit_last"); @@ -93,15 +95,14 @@ void PathManagerExample::manage_line(const Input & input, increment_indices(idx_a_, idx_b, idx_c, input, output); - if (orbit_last && (idx_a_ == num_waypoints_ - 1 || idx_a_ == num_waypoints_ -2)) - { + if (orbit_last && (idx_a_ == num_waypoints_ - 1 || idx_a_ == num_waypoints_ - 2)) { return; } Eigen::Vector3f w_im1(waypoints_[idx_a_].w); Eigen::Vector3f w_i(waypoints_[idx_b].w); Eigen::Vector3f w_ip1(waypoints_[idx_c].w); - + // Fill out data for straight line to the next point. output.flag = true; output.va_d = waypoints_[idx_a_].va_d; @@ -117,7 +118,7 @@ void PathManagerExample::manage_line(const Input & input, Eigen::Vector3f n_i = (q_im1 + q_i).normalized(); // Check if the planes were aligned and then handle the normal vector correctly. - if (n_i.isZero()){ + if (n_i.isZero()) { n_i = q_im1; } @@ -131,8 +132,7 @@ void PathManagerExample::manage_line(const Input & input, } } -void PathManagerExample::manage_fillet(const Input & input, - Output & output) +void PathManagerExample::manage_fillet(const Input & input, Output & output) { // For readability, declare the parameters that will be used in the function here bool orbit_last = params_.get_bool("orbit_last"); @@ -146,61 +146,61 @@ void PathManagerExample::manage_fillet(const Input & input, Eigen::Vector3f p; p << input.pn, input.pe, -input.h; - + // idx_a is the waypoint you are coming from. int idx_b; // Next waypoint. int idx_c; // Waypoint after next. - + increment_indices(idx_a_, idx_b, idx_c, input, output); - if (orbit_last && idx_a_ == num_waypoints_ - 1) - { + if (orbit_last && idx_a_ == num_waypoints_ - 1) { // TODO: check to see if this is the correct behavior. return; } Eigen::Vector3f w_im1(waypoints_[idx_a_].w); // Previous waypoint NED im1 means i-1 - Eigen::Vector3f w_i(waypoints_[idx_b].w); // Waypoint the aircraft is headed towards. - Eigen::Vector3f w_ip1(waypoints_[idx_c].w); // Waypoint after leaving waypoint idx_b. + Eigen::Vector3f w_i(waypoints_[idx_b].w); // Waypoint the aircraft is headed towards. + Eigen::Vector3f w_ip1(waypoints_[idx_c].w); // Waypoint after leaving waypoint idx_b. output.va_d = waypoints_[idx_a_].va_d; // Desired airspeed of this leg of the waypoints. - output.r[0] = w_im1(0); // See chapter 11 of the UAV book for more information. + output.r[0] = w_im1(0); // See chapter 11 of the UAV book for more information. output.r[1] = w_im1(1); // This is the point that is a point along the commanded path. output.r[2] = w_im1(2); // The vector pointing into the turn (vector pointing from previous waypoint to the next). - Eigen::Vector3f q_im1 = (w_i - w_im1); + Eigen::Vector3f q_im1 = (w_i - w_im1); float dist_w_im1 = q_im1.norm(); q_im1 = q_im1.normalized(); // The vector pointing out of the turn (vector points from next waypoint to the next next waypoint). - Eigen::Vector3f q_i = (w_ip1 - w_i); + Eigen::Vector3f q_i = (w_ip1 - w_i); float dist_w_ip1 = q_i.norm(); q_i = q_i.normalized(); float varrho = acosf(-q_im1.dot(q_i)); // Angle of the turn. - + // Check to see if filleting is possible for given waypoints. // Find closest dist to w_i float min_dist = std::min(dist_w_ip1, dist_w_im1); // Use varrho to find the distance to bisector from closest waypoint. - float max_r = min_dist * sinf(varrho/2.0); + float max_r = min_dist * sinf(varrho / 2.0); // If max_r (maximum radius possible for angle) is smaller than R_min, do line management. - if (R_min > max_r) - { + if (R_min > max_r) { // While in the too acute region, publish notice every 10 seconds. - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "Too acute an angle, using line management. Values, max_r: " << max_r << " R_min: " << R_min); + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Too acute an angle, using line management. Values, max_r: " << max_r << " R_min: " << R_min); manage_line(input, output); return; } Eigen::Vector3f z; switch (fil_state_) { - case FilletState::STRAIGHT: - { + case FilletState::STRAIGHT: { output.flag = true; // Indicate flying a straight path. - output.q[0] = q_im1(0); // Fly along vector into the turn the origin of the vector is r (set as previous waypoint above). + output.q[0] = q_im1( + 0); // Fly along vector into the turn the origin of the vector is r (set as previous waypoint above). output.q[1] = q_im1(1); output.q[2] = q_im1(2); output.c[0] = 1; // Fill rest of the data though it is not used. @@ -208,14 +208,18 @@ void PathManagerExample::manage_fillet(const Input & input, output.c[2] = 1; output.rho = 1; output.lamda = 1; - z = w_i - q_im1 * (R_min / tanf(varrho / 2.0)); // Point in plane where after passing through the aircraft should begin the turn. - - - if ((p - z).dot(q_im1) > 0) - { + z = w_i + - q_im1 + * (R_min + / tanf( + varrho + / 2.0)); // Point in plane where after passing through the aircraft should begin the turn. + + if ((p - z).dot(q_im1) > 0) { if (q_i == q_im1) // Check to see if the waypoint is directly between the next two. { - if (idx_a_ == num_waypoints_ - 1) idx_a_ = 0; + if (idx_a_ == num_waypoints_ - 1) + idx_a_ = 0; else idx_a_++; break; @@ -224,22 +228,30 @@ void PathManagerExample::manage_fillet(const Input & input, } break; } - case FilletState::TRANSITION: - { + case FilletState::TRANSITION: { output.flag = false; // Indicate that aircraft is following an orbit. - output.q[0] = q_i(0); // Load the message with the vector that will be follwed after the orbit. + output.q[0] = + q_i(0); // Load the message with the vector that will be follwed after the orbit. output.q[1] = q_i(1); output.q[2] = q_i(2); - Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(varrho / 2.0)); // Calculate the center of the orbit. - output.c[0] = c(0); // Load message with the center of the orbit. + Eigen::Vector3f c = w_i + - (q_im1 - q_i).normalized() + * (R_min / sinf(varrho / 2.0)); // Calculate the center of the orbit. + output.c[0] = c(0); // Load message with the center of the orbit. output.c[1] = c(1); output.c[2] = c(2); output.rho = R_min; // Command the orbit radius to be the minimum acheivable. - output.lamda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1); // Find the direction to orbit the point. - z = w_i + q_i * (R_min / tanf(varrho / 2.0)); // Find the point in the plane that once you pass through you should increment the indexes and follow a straight line. - - if (orbit_last && idx_a_ == num_waypoints_ - 2) - { + output.lamda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 + ? 1 + : -1); // Find the direction to orbit the point. + z = w_i + + q_i + * (R_min + / tanf( + varrho + / 2.0)); // Find the point in the plane that once you pass through you should increment the indexes and follow a straight line. + + if (orbit_last && idx_a_ == num_waypoints_ - 2) { idx_a_++; fil_state_ = FilletState::STRAIGHT; break; @@ -250,21 +262,32 @@ void PathManagerExample::manage_fillet(const Input & input, } break; } - case FilletState::ORBIT: - { + case FilletState::ORBIT: { output.flag = false; // Indicate that aircraft is following an orbit. - output.q[0] = q_i(0); // Load the message with the vector that will be follwed after the orbit. + output.q[0] = + q_i(0); // Load the message with the vector that will be follwed after the orbit. output.q[1] = q_i(1); output.q[2] = q_i(2); - Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(varrho / 2.0)); // Calculate the center of the orbit. - output.c[0] = c(0); // Load message with the center of the orbit. + Eigen::Vector3f c = w_i + - (q_im1 - q_i).normalized() + * (R_min / sinf(varrho / 2.0)); // Calculate the center of the orbit. + output.c[0] = c(0); // Load message with the center of the orbit. output.c[1] = c(1); output.c[2] = c(2); output.rho = R_min; // Command the orbit radius to be the minimum acheivable. - output.lamda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1); // Find the direction to orbit the point. TODO change this to the orbit_direction. - z = w_i + q_i * (R_min / tanf(varrho / 2.0)); // Find the point in the plane that once you pass through you should increment the indexes and follow a straight line. + output.lamda = + ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 + ? 1 + : -1); // Find the direction to orbit the point. TODO change this to the orbit_direction. + z = w_i + + q_i + * (R_min + / tanf( + varrho + / 2.0)); // Find the point in the plane that once you pass through you should increment the indexes and follow a straight line. if ((p - z).dot(q_i) > 0) { // Check to see if passed through plane. - if (idx_a_ == num_waypoints_ - 1) idx_a_ = 0; + if (idx_a_ == num_waypoints_ - 1) + idx_a_ = 0; else idx_a_++; fil_state_ = FilletState::STRAIGHT; @@ -274,8 +297,7 @@ void PathManagerExample::manage_fillet(const Input & input, } } -void PathManagerExample::manage_dubins(const Input & input, - Output & output) +void PathManagerExample::manage_dubins(const Input & input, Output & output) { // For readability, declare the parameters that will be used in the function here double R_min = params_.get_double("R_min"); @@ -417,7 +439,8 @@ Eigen::Matrix3f PathManagerExample::rotz(float theta) float PathManagerExample::mo(float in) { float val; - if (in > 0) val = fmod(in, 2.0 * M_PI_F); + if (in > 0) + val = fmod(in, 2.0 * M_PI_F); else { float n = floorf(in / 2.0 / M_PI_F); val = in - n * 2.0 * M_PI_F; @@ -426,7 +449,7 @@ float PathManagerExample::mo(float in) } void PathManagerExample::dubins_parameters(const Waypoint start_node, const Waypoint end_node, - float R) + float R) { 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])); @@ -475,7 +498,8 @@ void PathManagerExample::dubins_parameters(const Waypoint start_node, const Wayp ell = (cle - crs).norm(); theta = atan2f(cle(1) - crs(1), cle(0) - crs(0)); float L2; - if (2.0 * R > ell) L2 = 9999.0f; + if (2.0 * R > ell) + L2 = 9999.0f; else { theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell); L2 = sqrtf(ell * ell - 4.0 * R * R) @@ -487,7 +511,8 @@ void PathManagerExample::dubins_parameters(const Waypoint start_node, const Wayp ell = (cre - cls).norm(); theta = atan2f(cre(1) - cls(1), cre(0) - cls(0)); float L3; - if (2.0 * R > ell) L3 = 9999.0f; + if (2.0 * R > ell) + L3 = 9999.0f; else { theta2 = acosf(2.0 * R / ell); L3 = sqrtf(ell * ell - 4 * R * R) @@ -512,7 +537,8 @@ void PathManagerExample::dubins_parameters(const Waypoint start_node, const Wayp dubins_path_.L = L3; idx = 3; } - if (L4 < dubins_path_.L) { dubins_path_.L = L4; + if (L4 < dubins_path_.L) { + dubins_path_.L = L4; idx = 4; } @@ -571,15 +597,11 @@ void PathManagerExample::dubins_parameters(const Waypoint start_node, const Wayp } } -void PathManagerExample::declare_parameters() -{ - params_.declare_bool("orbit_last", false); -} +void PathManagerExample::declare_parameters() { params_.declare_bool("orbit_last", false); } int PathManagerExample::orbit_direction(float pn, float pe, float chi, float c_n, float c_e) { - if (orbit_dir_ != 0) - { + if (orbit_dir_ != 0) { return orbit_dir_; } @@ -594,24 +616,23 @@ int PathManagerExample::orbit_direction(float pn, float pe, float chi, float c_n Eigen::Vector3f course; course << sinf(chi), cosf(chi), 0.0; - if (d.cross(course)(2) >= 0 ) - { + if (d.cross(course)(2) >= 0) { orbit_dir_ = 1; return 1; } - + orbit_dir_ = -1; return -1; } -void PathManagerExample::increment_indices(int & idx_a, int & idx_b, int & idx_c, const Input & input, Output & output) +void PathManagerExample::increment_indices(int & idx_a, int & idx_b, int & idx_c, + const Input & input, Output & output) { bool orbit_last = params_.get_bool("orbit_last"); double R_min = params_.get_double("R_min"); - if (temp_waypoint_ && idx_a_ == 1) - { + if (temp_waypoint_ && idx_a_ == 1) { waypoints_.erase(waypoints_.begin()); num_waypoints_--; idx_a_ = 0; @@ -622,7 +643,7 @@ void PathManagerExample::increment_indices(int & idx_a, int & idx_b, int & idx_c } if (idx_a == num_waypoints_ - 1) { // The logic for if it is the last waypoint. - + // If it is the last waypoint, and we orbit the last waypoint, construct the command. if (orbit_last) { output.flag = false; @@ -637,15 +658,20 @@ void PathManagerExample::increment_indices(int & idx_a, int & idx_b, int & idx_c output.q[1] = 0.0; output.q[2] = 0.0; output.rho = R_min; - output.lamda = orbit_direction(input.pn, input.pe, input.chi, output.c[0], output.c[1]); // Calculate the most conveinent orbit direction of that point. - idx_b = 0; // reset the path and loop the waypoints again. + output.lamda = orbit_direction( + input.pn, input.pe, input.chi, output.c[0], + output.c[1]); // Calculate the most conveinent orbit direction of that point. + idx_b = 0; // reset the path and loop the waypoints again. idx_c = 1; return; } idx_b = 0; // reset the path and loop the waypoints again. idx_c = 1; - } else if (idx_a == num_waypoints_ - 2) { // If the second to last waypoint, appropriately handle the wrapping of waypoints. + } else if ( + idx_a + == num_waypoints_ + - 2) { // If the second to last waypoint, appropriately handle the wrapping of waypoints. idx_b = num_waypoints_ - 1; idx_c = 0; } else { // Increment the indices of the waypoints. diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index b11bf85..50ca511 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -10,9 +10,9 @@ #include #include "param_manager.hpp" -#include "rosplane_msgs/srv/add_waypoint.hpp" #include "rosplane_msgs/msg/state.hpp" #include "rosplane_msgs/msg/waypoint.hpp" +#include "rosplane_msgs/srv/add_waypoint.hpp" #include "path_planner.hpp" @@ -23,20 +23,22 @@ namespace rosplane { PathPlanner::PathPlanner() - : Node("path_planner"), params_(this) + : 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_); + waypoint_publisher_ = + this->create_publisher("waypoint_path", qos_transient_local_10_); next_waypoint_service_ = this->create_service( "publish_next_waypoint", std::bind(&PathPlanner::publish_next_waypoint, this, _1, _2)); - + add_waypoint_service_ = this->create_service( "add_waypoint", std::bind(&PathPlanner::update_path, this, _1, _2)); - + clear_waypoint_service_ = this->create_service( "clear_waypoints", std::bind(&PathPlanner::clear_path_callback, this, _1, _2)); @@ -45,9 +47,9 @@ PathPlanner::PathPlanner() load_mission_service_ = this->create_service( "load_mission_from_file", std::bind(&PathPlanner::load_mission, this, _1, _2)); - - state_subscription_ = this->create_subscription("estimated_state", 10, - std::bind(&PathPlanner::state_callback, this, _1)); + + state_subscription_ = this->create_subscription( + "estimated_state", 10, std::bind(&PathPlanner::state_callback, this, _1)); // Set the parameter callback, for when parameters are changed. parameter_callback_handle_ = this->add_on_set_parameters_callback( @@ -69,18 +71,24 @@ PathPlanner::PathPlanner() PathPlanner::~PathPlanner() {} -void PathPlanner::publish_initial_waypoints() { - int num_waypoints_to_publish_at_start = this->get_parameter("num_waypoints_to_publish_at_start").as_int(); +void PathPlanner::publish_initial_waypoints() +{ + int num_waypoints_to_publish_at_start = + this->get_parameter("num_waypoints_to_publish_at_start").as_int(); - RCLCPP_INFO_STREAM_ONCE(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()) { + while (num_waypoints_published_ < num_waypoints_to_publish_at_start + && num_waypoints_published_ < (int) wps.size()) { waypoint_publish(); } } -void PathPlanner::state_callback(const rosplane_msgs::msg::State & msg) { +void PathPlanner::state_callback(const rosplane_msgs::msg::State & msg) +{ // 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) { @@ -91,7 +99,7 @@ void PathPlanner::state_callback(const rosplane_msgs::msg::State & msg) { } bool PathPlanner::publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) + const std_srvs::srv::Trigger::Response::SharedPtr & res) { // Publish the next waypoint, if available if (num_waypoints_published_ < (int) wps.size()) { @@ -103,8 +111,7 @@ bool PathPlanner::publish_next_waypoint(const std_srvs::srv::Trigger::Request::S res->success = true; return true; - } - else { + } else { RCLCPP_ERROR_STREAM(this->get_logger(), "No waypoints left to publish! Add more waypoints"); res->success = false; return false; @@ -122,14 +129,15 @@ void PathPlanner::waypoint_publish() } bool PathPlanner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::SharedPtr & req, - const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res) { - + const rosplane_msgs::srv::AddWaypoint::Response::SharedPtr & res) +{ + rosplane_msgs::msg::Waypoint new_waypoint; rclcpp::Time now = this->get_clock()->now(); new_waypoint.header.stamp = now; - + // Create a string flag for the debug message (defaults to "LLA") std::string lla_or_ned = "LLA"; @@ -139,8 +147,7 @@ bool PathPlanner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::Sh new_waypoint.w[0] = ned[0]; new_waypoint.w[1] = ned[1]; new_waypoint.w[2] = ned[2]; - } - else { + } else { new_waypoint.w = req->w; lla_or_ned = "NED"; } @@ -157,8 +164,7 @@ bool PathPlanner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::Sh wps.insert(wps.begin() + num_waypoints_published_, new_waypoint); waypoint_publish(); res->message = "Adding " + lla_or_ned + " waypoint was successful! Waypoint published."; - } - else { + } else { wps.push_back(new_waypoint); res->message = "Adding " + lla_or_ned + " waypoint was successful!"; } @@ -170,14 +176,16 @@ bool PathPlanner::update_path(const rosplane_msgs::srv::AddWaypoint::Request::Sh } bool PathPlanner::clear_path_callback(const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) { + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ clear_path(); res->success = true; return true; } -void PathPlanner::clear_path() { +void PathPlanner::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 @@ -190,20 +198,22 @@ void PathPlanner::clear_path() { } bool PathPlanner::print_path(const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) { + const std_srvs::srv::Trigger::Response::SharedPtr & res) +{ std::stringstream output; - + output << "Printing waypoints..."; - for (int i=0; i < (int) wps.size(); ++i) { + for (int i = 0; i < (int) wps.size(); ++i) { rosplane_msgs::msg::Waypoint wp = wps[i]; output << std::endl << "----- WAYPOINT " << i << " -----" << std::endl; - + if (wp.lla) { - output << "Position (LLA): [" << wp.w[0] << ", " << wp.w[1] << ", " << wp.w[2] << "]" << std::endl; - } - else { - output << "Position (NED, meters): [" << wp.w[0] << ", " << wp.w[1] << ", " << wp.w[2] << "]" << std::endl; + output << "Position (LLA): [" << wp.w[0] << ", " << wp.w[1] << ", " << wp.w[2] << "]" + << std::endl; + } else { + 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; @@ -219,20 +229,22 @@ bool PathPlanner::print_path(const std_srvs::srv::Trigger::Request::SharedPtr & } bool PathPlanner::load_mission(const rosflight_msgs::srv::ParamFile::Request::SharedPtr & req, - const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res) { + const rosflight_msgs::srv::ParamFile::Response::SharedPtr & res) +{ clear_path(); res->success = load_mission_from_file(req->filename); publish_initial_waypoints(); return true; } -bool PathPlanner::load_mission_from_file(const std::string& filename) { +bool PathPlanner::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); - for (YAML::const_iterator it=root.begin(); it!=root.end(); ++it) { + for (YAML::const_iterator it = root.begin(); it != root.end(); ++it) { YAML::Node wp = it->second; rosplane_msgs::msg::Waypoint new_wp; @@ -255,14 +267,14 @@ bool PathPlanner::load_mission_from_file(const std::string& filename) { } return true; - } - catch (...) { + } catch (...) { RCLCPP_ERROR_STREAM(this->get_logger(), "Error while parsing mission YAML file! Check inputs"); return false; } } -std::array PathPlanner::lla2ned(std::array lla) { +std::array PathPlanner::lla2ned(std::array lla) +{ double lat1 = lla[0]; double lon1 = lla[1]; double alt1 = lla[2]; @@ -278,10 +290,13 @@ std::array PathPlanner::lla2ned(std::array lla) { // 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"); + RCLCPP_WARN_STREAM(this->get_logger(), + "NED position set to [" + << n << "," << e << "," << d + << "]! Waypoints may be incorrect. Check GPS health"); } - return std::array {n, e, d}; + return std::array{n, e, d}; } rcl_interfaces::msg::SetParametersResult @@ -293,8 +308,7 @@ PathPlanner::parametersCallback(const std::vector & parameter // Set parameters in the param_manager object bool success = params_.set_parameters_callback(parameters); - if (success) - { + if (success) { result.successful = true; result.reason = "success"; } @@ -302,7 +316,8 @@ PathPlanner::parametersCallback(const std::vector & parameter return result; } -void PathPlanner::declare_parameters() { +void PathPlanner::declare_parameters() +{ params_.declare_int("num_waypoints_to_publish_at_start", 3); } diff --git a/rosplane_extra/include/input_mapper.hpp b/rosplane_extra/include/input_mapper.hpp index 4c34f67..d93f90a 100644 --- a/rosplane_extra/include/input_mapper.hpp +++ b/rosplane_extra/include/input_mapper.hpp @@ -69,7 +69,7 @@ class InputMapper : public rclcpp::Node */ bool param_change_pending_; - /** + /** * Keeps track of previous time of the last controller command sent for rate control. */ rclcpp::Time last_command_time_; @@ -78,7 +78,7 @@ class InputMapper : public rclcpp::Node * This publisher publishes the mapped controller commands for the autopilot. */ rclcpp::Publisher::SharedPtr - mapped_controller_commands_pub_; + mapped_controller_commands_pub_; /** * This publisher publishes the mapped command for the firmware. */ @@ -203,9 +203,8 @@ class InputMapper : public rclcpp::Node * @param request The request object for the service. * @param response The response object for the service. */ - void path_follower_mode_callback( - const std::shared_ptr request, - std::shared_ptr response); + void path_follower_mode_callback(const std::shared_ptr request, + std::shared_ptr response); /** * This function is called when the altitude course airspeed control mode service is called, * setting the input modes to altitude, course, and airspeed rate control. @@ -214,8 +213,8 @@ class InputMapper : public rclcpp::Node * @param response The response object for the service. */ void altitude_course_airspeed_control_mode_callback( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); /** * This function is called when the angle control mode service is called, setting the input * modes to angle control mode. @@ -223,9 +222,8 @@ class InputMapper : public rclcpp::Node * @param request The request object for the service. * @param response The response object for the service. */ - void angle_control_mode_callback( - const std::shared_ptr request, - std::shared_ptr response); + void angle_control_mode_callback(const std::shared_ptr request, + std::shared_ptr response); /** * This function is called when the RC passthrough mode service is called, setting the input * modes to RC passthrough. @@ -233,9 +231,8 @@ class InputMapper : public rclcpp::Node * @param request The request object for the service. * @param response The response object for the service. */ - void rc_passthrough_mode_callback( - const std::shared_ptr request, - std::shared_ptr response); + void rc_passthrough_mode_callback(const std::shared_ptr request, + std::shared_ptr response); /// Parameters stuff @@ -258,8 +255,7 @@ class InputMapper : public rclcpp::Node */ rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector & parameters); - }; -} +} // namespace rosplane #endif //INPUT_MAPPER_HPP diff --git a/rosplane_extra/src/input_mapper.cpp b/rosplane_extra/src/input_mapper.cpp index 5520c20..b628754 100644 --- a/rosplane_extra/src/input_mapper.cpp +++ b/rosplane_extra/src/input_mapper.cpp @@ -6,30 +6,26 @@ using std::placeholders::_2; namespace rosplane { -InputMapper::InputMapper() : - Node("input_mapper"), - roll_override_(false), - pitch_override_(false), - param_change_pending_(false), - params_(this) +InputMapper::InputMapper() + : Node("input_mapper") + , roll_override_(false) + , pitch_override_(false) + , param_change_pending_(false) + , params_(this) { - mapped_controller_commands_pub_ = this->create_publisher( - "mapped_controller_command", 10); - mapped_command_pub_ = this->create_publisher( - "mapped_command", 10); + mapped_controller_commands_pub_ = + this->create_publisher("mapped_controller_command", 10); + mapped_command_pub_ = this->create_publisher("mapped_command", 10); controller_commands_sub_ = this->create_subscription( "input_controller_command", 10, std::bind(&InputMapper::controller_commands_callback, this, _1)); command_sub_ = this->create_subscription( - "input_command", 10, - std::bind(&InputMapper::command_callback, this, _1)); + "input_command", 10, std::bind(&InputMapper::command_callback, this, _1)); rc_raw_sub_ = this->create_subscription( - "rc_raw", 10, - std::bind(&InputMapper::rc_raw_callback, this, _1)); + "rc_raw", 10, std::bind(&InputMapper::rc_raw_callback, this, _1)); state_sub_ = this->create_subscription( - "estimated_state", 10, - std::bind(&InputMapper::state_callback, this, _1)); + "estimated_state", 10, std::bind(&InputMapper::state_callback, this, _1)); set_param_client_ = this->create_client( "/autopilot/set_parameters", rmw_qos_profile_services_default); @@ -38,14 +34,17 @@ InputMapper::InputMapper() : set_param_timer_->cancel(); path_follower_mode_service_ = this->create_service( - "/input_mapper/set_path_follower_mode", std::bind(&InputMapper::path_follower_mode_callback, this, _1, _2)); + "/input_mapper/set_path_follower_mode", + std::bind(&InputMapper::path_follower_mode_callback, this, _1, _2)); altitude_course_airspeed_control_mode_service_ = this->create_service( "/input_mapper/set_altitude_course_airspeed_control_mode", std::bind(&InputMapper::altitude_course_airspeed_control_mode_callback, this, _1, _2)); angle_control_mode_service_ = this->create_service( - "/input_mapper/set_angle_control_mode", std::bind(&InputMapper::angle_control_mode_callback, this, _1, _2)); + "/input_mapper/set_angle_control_mode", + std::bind(&InputMapper::angle_control_mode_callback, this, _1, _2)); rc_passthrough_mode_service_ = this->create_service( - "/input_mapper/set_rc_passthrough_mode", std::bind(&InputMapper::rc_passthrough_mode_callback, this, _1, _2)); + "/input_mapper/set_rc_passthrough_mode", + std::bind(&InputMapper::rc_passthrough_mode_callback, this, _1, _2)); last_command_time_ = this->now(); mapped_controller_commands_msg_ = std::make_shared(); @@ -65,8 +64,8 @@ InputMapper::InputMapper() : params_.declare_double("rc_airspeed_rate", 1.0); // Set the parameter callback, for when parameters are changed. - parameter_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&InputMapper::parametersCallback, this, _1)); + parameter_callback_handle_ = + this->add_on_set_parameters_callback(std::bind(&InputMapper::parametersCallback, this, _1)); } void InputMapper::set_param_timer_callback() @@ -146,7 +145,8 @@ void InputMapper::set_pitch_override(bool pitch_override) param_change_pending_ = true; } -void InputMapper::controller_commands_callback(const rosplane_msgs::msg::ControllerCommands::SharedPtr msg) +void InputMapper::controller_commands_callback( + const rosplane_msgs::msg::ControllerCommands::SharedPtr msg) { std::string aileron_input = params_.get_string("aileron_input"); std::string elevator_input = params_.get_string("elevator_input"); @@ -169,22 +169,23 @@ void InputMapper::controller_commands_callback(const rosplane_msgs::msg::Control mapped_controller_commands_msg_->chi_c = msg->chi_c; } else if (aileron_input == "rc_course") { set_roll_override(false); - mapped_controller_commands_msg_->chi_c += norm_aileron * - params_.get_double("rc_course_rate") * - elapsed_time; - mapped_controller_commands_msg_->chi_c = mapped_controller_commands_msg_->chi_c - - floor((mapped_controller_commands_msg_->chi_c - state_msg_->chi) / (2 * M_PI) + 0.5) * - 2 * M_PI; + mapped_controller_commands_msg_->chi_c += + norm_aileron * params_.get_double("rc_course_rate") * elapsed_time; + mapped_controller_commands_msg_->chi_c = mapped_controller_commands_msg_->chi_c + - floor((mapped_controller_commands_msg_->chi_c - state_msg_->chi) / (2 * M_PI) + 0.5) * 2 + * M_PI; } else if (aileron_input == "rc_roll_angle") { set_roll_override(true); - mapped_controller_commands_msg_->phi_c = norm_aileron * params_.get_double("rc_roll_angle_min_max"); + mapped_controller_commands_msg_->phi_c = + norm_aileron * params_.get_double("rc_roll_angle_min_max"); mapped_controller_commands_msg_->chi_c = state_msg_->chi; } else if (aileron_input == "rc_aileron") { mapped_controller_commands_msg_->chi_c = state_msg_->chi; } else { - RCLCPP_ERROR(this->get_logger(), "Invalid aileron input type: %s. Valid options are " - "path_follower, rc_course, rc_roll_angle, and rc_aileron." - "Setting to path_follower.", + RCLCPP_ERROR(this->get_logger(), + "Invalid aileron input type: %s. Valid options are " + "path_follower, rc_course, rc_roll_angle, and rc_aileron." + "Setting to path_follower.", aileron_input.c_str()); params_.set_string("aileron_input", "path_follower"); } @@ -195,19 +196,20 @@ void InputMapper::controller_commands_callback(const rosplane_msgs::msg::Control mapped_controller_commands_msg_->h_c = msg->h_c; } else if (elevator_input == "rc_altitude") { set_pitch_override(false); - mapped_controller_commands_msg_->h_c += norm_elevator * - params_.get_double("rc_altitude_rate") * - elapsed_time; + mapped_controller_commands_msg_->h_c += + norm_elevator * params_.get_double("rc_altitude_rate") * elapsed_time; } else if (elevator_input == "rc_pitch_angle") { set_pitch_override(true); - mapped_controller_commands_msg_->theta_c = norm_elevator * params_.get_double("rc_pitch_angle_min_max"); + mapped_controller_commands_msg_->theta_c = + norm_elevator * params_.get_double("rc_pitch_angle_min_max"); mapped_controller_commands_msg_->h_c = -state_msg_->position[2]; } else if (elevator_input == "rc_elevator") { mapped_controller_commands_msg_->h_c = -state_msg_->position[2]; } else { - RCLCPP_ERROR(this->get_logger(), "Invalid elevator input type: %s. Valid options are " - "path_follower, rc_altitude, rc_pitch_angle, and rc_elevator. " - "Setting to path_follower.", + RCLCPP_ERROR(this->get_logger(), + "Invalid elevator input type: %s. Valid options are " + "path_follower, rc_altitude, rc_pitch_angle, and rc_elevator. " + "Setting to path_follower.", elevator_input.c_str()); params_.set_string("elevator_input", "path_follower"); } @@ -216,15 +218,15 @@ void InputMapper::controller_commands_callback(const rosplane_msgs::msg::Control if (throttle_input == "path_follower") { mapped_controller_commands_msg_->va_c = msg->va_c; } else if (throttle_input == "rc_airspeed") { - mapped_controller_commands_msg_->va_c += norm_throttle * - params_.get_double("rc_airspeed_rate") * - elapsed_time; + mapped_controller_commands_msg_->va_c += + norm_throttle * params_.get_double("rc_airspeed_rate") * elapsed_time; } else if (throttle_input == "rc_throttle") { mapped_controller_commands_msg_->va_c = state_msg_->va; } else { - RCLCPP_ERROR(this->get_logger(), "Invalid throttle input type: %s. Valid options are " - "path_follower, rc_airspeed and rc_throttle. Setting to " - "path_follower.", + RCLCPP_ERROR(this->get_logger(), + "Invalid throttle input type: %s. Valid options are " + "path_follower, rc_airspeed and rc_throttle. Setting to " + "path_follower.", throttle_input.c_str()); params_.set_string("throttle_input", "path_follower"); } @@ -233,8 +235,9 @@ void InputMapper::controller_commands_callback(const rosplane_msgs::msg::Control if (rudder_input == "yaw_damper") { } else if (rudder_input == "rc_rudder") { } else { - RCLCPP_ERROR(this->get_logger(), "Invalid rudder input type: %s. Valid options are " - "yaw_damper and rc_rudder. Setting to yaw_damper.", + RCLCPP_ERROR(this->get_logger(), + "Invalid rudder input type: %s. Valid options are " + "yaw_damper and rc_rudder. Setting to yaw_damper.", rudder_input.c_str()); params_.set_string("rudder_input", "yaw_damper"); } @@ -323,8 +326,7 @@ InputMapper::parametersCallback(const std::vector & parameter result.reason = "success"; bool success = params_.set_parameters_callback(parameters); - if (!success) - { + if (!success) { result.successful = false; result.reason = "One of the parameters given does not is not a parameter of the input mapper node."; diff --git a/rosplane_gcs/src/rviz_waypoint_publisher.cpp b/rosplane_gcs/src/rviz_waypoint_publisher.cpp index f3b2908..95b4809 100644 --- a/rosplane_gcs/src/rviz_waypoint_publisher.cpp +++ b/rosplane_gcs/src/rviz_waypoint_publisher.cpp @@ -21,246 +21,256 @@ namespace rosplane_gcs class RvizWaypointPublisher : public rclcpp::Node { public: - RvizWaypointPublisher(); - ~RvizWaypointPublisher(); + RvizWaypointPublisher(); + ~RvizWaypointPublisher(); private: - rclcpp::Publisher::SharedPtr rviz_wp_pub_; - rclcpp::Publisher::SharedPtr rviz_mesh_pub_; - rclcpp::Publisher::SharedPtr rviz_aircraft_path_pub_; - rclcpp::Subscription::SharedPtr waypoint_sub_; - rclcpp::Subscription::SharedPtr vehicle_state_sub_; - - std::unique_ptr aircraft_tf2_broadcaster_; - - void new_wp_callback(const rosplane_msgs::msg::Waypoint & wp); - void state_update_callback(const rosplane_msgs::msg::State & state); - void update_list(); - void update_mesh(); - void update_aircraft_history(); - - rosplane_msgs::msg::State vehicle_state_; - - // Persistent rviz markers - visualization_msgs::msg::Marker line_list_; - std::vector line_points_; - visualization_msgs::msg::Marker aircraft_; - visualization_msgs::msg::Marker aircraft_history_; - std::vector aircraft_history_points_; - - int num_wps_; - int i_; + rclcpp::Publisher::SharedPtr rviz_wp_pub_; + rclcpp::Publisher::SharedPtr rviz_mesh_pub_; + rclcpp::Publisher::SharedPtr rviz_aircraft_path_pub_; + rclcpp::Subscription::SharedPtr waypoint_sub_; + rclcpp::Subscription::SharedPtr vehicle_state_sub_; + + std::unique_ptr aircraft_tf2_broadcaster_; + + void new_wp_callback(const rosplane_msgs::msg::Waypoint & wp); + void state_update_callback(const rosplane_msgs::msg::State & state); + void update_list(); + void update_mesh(); + void update_aircraft_history(); + + rosplane_msgs::msg::State vehicle_state_; + + // Persistent rviz markers + visualization_msgs::msg::Marker line_list_; + std::vector line_points_; + visualization_msgs::msg::Marker aircraft_; + visualization_msgs::msg::Marker aircraft_history_; + std::vector aircraft_history_points_; + + int num_wps_; + int i_; }; RvizWaypointPublisher::RvizWaypointPublisher() - : Node("rviz_waypoint_publisher") { - - rclcpp::QoS qos_transient_local_20_(20); - qos_transient_local_20_.transient_local(); - rviz_wp_pub_ = this->create_publisher("rviz/waypoint", qos_transient_local_20_); - rviz_mesh_pub_ = this->create_publisher("rviz/mesh", 5); - rviz_aircraft_path_pub_ = this->create_publisher("rviz/mesh_path", 5); - waypoint_sub_ = this->create_subscription("waypoint_path", qos_transient_local_20_, - std::bind(&RvizWaypointPublisher::new_wp_callback, this, _1)); - vehicle_state_sub_ = this->create_subscription("estimated_state", 10, - std::bind(&RvizWaypointPublisher::state_update_callback, this, _1)); - - aircraft_tf2_broadcaster_ = std::make_unique(*this); - - // Initialize aircraft - aircraft_.header.frame_id = "stl_frame"; - aircraft_.ns = "vehicle"; - aircraft_.id = 0; - aircraft_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; - aircraft_.mesh_resource = "package://rosplane_gcs/resource/skyhunter.dae"; - aircraft_.mesh_use_embedded_materials = false; - aircraft_.action = visualization_msgs::msg::Marker::ADD; - aircraft_.pose.position.x = 0.0; - aircraft_.pose.position.y = 0.0; - aircraft_.pose.position.z = 0.0; - aircraft_.pose.orientation.x = 0.0; - aircraft_.pose.orientation.y = 0.0; - aircraft_.pose.orientation.z = 0.0; - aircraft_.pose.orientation.w = 1.0; - aircraft_.scale.x = 5.0; - aircraft_.scale.y = 5.0; - aircraft_.scale.z = 5.0; - aircraft_.color.r = 0.67f; - aircraft_.color.g = 0.67f; - aircraft_.color.b = 0.67f; - aircraft_.color.a = 1.0; + : Node("rviz_waypoint_publisher") +{ - num_wps_ = 0; - i_ = 0; + rclcpp::QoS qos_transient_local_20_(20); + qos_transient_local_20_.transient_local(); + rviz_wp_pub_ = this->create_publisher("rviz/waypoint", + qos_transient_local_20_); + rviz_mesh_pub_ = this->create_publisher("rviz/mesh", 5); + rviz_aircraft_path_pub_ = + this->create_publisher("rviz/mesh_path", 5); + waypoint_sub_ = this->create_subscription( + "waypoint_path", qos_transient_local_20_, + std::bind(&RvizWaypointPublisher::new_wp_callback, this, _1)); + vehicle_state_sub_ = this->create_subscription( + "estimated_state", 10, std::bind(&RvizWaypointPublisher::state_update_callback, this, _1)); + + aircraft_tf2_broadcaster_ = std::make_unique(*this); + + // Initialize aircraft + aircraft_.header.frame_id = "stl_frame"; + aircraft_.ns = "vehicle"; + aircraft_.id = 0; + aircraft_.type = visualization_msgs::msg::Marker::MESH_RESOURCE; + aircraft_.mesh_resource = "package://rosplane_gcs/resource/skyhunter.dae"; + aircraft_.mesh_use_embedded_materials = false; + aircraft_.action = visualization_msgs::msg::Marker::ADD; + aircraft_.pose.position.x = 0.0; + aircraft_.pose.position.y = 0.0; + aircraft_.pose.position.z = 0.0; + aircraft_.pose.orientation.x = 0.0; + aircraft_.pose.orientation.y = 0.0; + aircraft_.pose.orientation.z = 0.0; + aircraft_.pose.orientation.w = 1.0; + aircraft_.scale.x = 5.0; + aircraft_.scale.y = 5.0; + aircraft_.scale.z = 5.0; + aircraft_.color.r = 0.67f; + aircraft_.color.g = 0.67f; + aircraft_.color.b = 0.67f; + aircraft_.color.a = 1.0; + + num_wps_ = 0; + i_ = 0; } RvizWaypointPublisher::~RvizWaypointPublisher() {} -void RvizWaypointPublisher::new_wp_callback(const rosplane_msgs::msg::Waypoint & wp) { - visualization_msgs::msg::Marker new_marker; - - RCLCPP_INFO_STREAM(this->get_logger(), wp.lla); - - if (wp.clear_wp_list) { - rclcpp::Time now = this->get_clock()->now(); - // Publish one for each ns - new_marker.header.stamp = now; - new_marker.header.frame_id = "NED"; - new_marker.ns = "wp"; - new_marker.id = 0; - new_marker.action = visualization_msgs::msg::Marker::DELETEALL; - rviz_wp_pub_->publish(new_marker); - new_marker.ns = "text"; - rviz_wp_pub_->publish(new_marker); - new_marker.ns = "wp_path"; - rviz_wp_pub_->publish(new_marker); - - // Clear line list - line_points_.clear(); - - num_wps_ = 0; - return; - } - - // Create marker +void RvizWaypointPublisher::new_wp_callback(const rosplane_msgs::msg::Waypoint & wp) +{ + visualization_msgs::msg::Marker new_marker; + + RCLCPP_INFO_STREAM(this->get_logger(), wp.lla); + + if (wp.clear_wp_list) { rclcpp::Time now = this->get_clock()->now(); + // Publish one for each ns new_marker.header.stamp = now; new_marker.header.frame_id = "NED"; new_marker.ns = "wp"; - new_marker.id = num_wps_; - new_marker.type = visualization_msgs::msg::Marker::SPHERE; - new_marker.action = visualization_msgs::msg::Marker::ADD; - new_marker.pose.position.x = wp.w[0]; - new_marker.pose.position.y = wp.w[1]; - new_marker.pose.position.z = wp.w[2]; - new_marker.scale.x = SCALE; - new_marker.scale.y = SCALE; - new_marker.scale.z = SCALE; - new_marker.color.r = 1.0f; - new_marker.color.g = 0.0f; - new_marker.color.b = 0.0f; - new_marker.color.a = 1.0; - - // Add point to line list - geometry_msgs::msg::Point new_p; - new_p.x = wp.w[0]; - new_p.y = wp.w[1]; - new_p.z = wp.w[2]; - line_points_.push_back(new_p); - update_list(); - - // Add Text label to marker - visualization_msgs::msg::Marker new_text; - new_text.header.stamp = now; - new_text.header.frame_id = "NED"; - new_text.ns = "text"; - new_text.id = num_wps_; - new_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - new_text.action = visualization_msgs::msg::Marker::ADD; - new_text.pose.position.x = wp.w[0]; - new_text.pose.position.y = wp.w[1]; - new_text.pose.position.z = wp.w[2] - SCALE - 1.0; - new_text.scale.z = TEXT_SCALE; - new_text.color.r = 0.0f; - new_text.color.g = 0.0f; - new_text.color.b = 0.0f; - new_text.color.a = 1.0; - new_text.text = std::to_string(num_wps_); - + new_marker.id = 0; + new_marker.action = visualization_msgs::msg::Marker::DELETEALL; + rviz_wp_pub_->publish(new_marker); + new_marker.ns = "text"; + rviz_wp_pub_->publish(new_marker); + new_marker.ns = "wp_path"; rviz_wp_pub_->publish(new_marker); - rviz_wp_pub_->publish(line_list_); - rviz_wp_pub_->publish(new_text); - ++num_wps_; + // Clear line list + line_points_.clear(); + + num_wps_ = 0; + return; + } + + // Create marker + rclcpp::Time now = this->get_clock()->now(); + new_marker.header.stamp = now; + new_marker.header.frame_id = "NED"; + new_marker.ns = "wp"; + new_marker.id = num_wps_; + new_marker.type = visualization_msgs::msg::Marker::SPHERE; + new_marker.action = visualization_msgs::msg::Marker::ADD; + new_marker.pose.position.x = wp.w[0]; + new_marker.pose.position.y = wp.w[1]; + new_marker.pose.position.z = wp.w[2]; + new_marker.scale.x = SCALE; + new_marker.scale.y = SCALE; + new_marker.scale.z = SCALE; + new_marker.color.r = 1.0f; + new_marker.color.g = 0.0f; + new_marker.color.b = 0.0f; + new_marker.color.a = 1.0; + + // Add point to line list + geometry_msgs::msg::Point new_p; + new_p.x = wp.w[0]; + new_p.y = wp.w[1]; + new_p.z = wp.w[2]; + line_points_.push_back(new_p); + update_list(); + + // Add Text label to marker + visualization_msgs::msg::Marker new_text; + new_text.header.stamp = now; + new_text.header.frame_id = "NED"; + new_text.ns = "text"; + new_text.id = num_wps_; + new_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + new_text.action = visualization_msgs::msg::Marker::ADD; + new_text.pose.position.x = wp.w[0]; + new_text.pose.position.y = wp.w[1]; + new_text.pose.position.z = wp.w[2] - SCALE - 1.0; + new_text.scale.z = TEXT_SCALE; + new_text.color.r = 0.0f; + new_text.color.g = 0.0f; + new_text.color.b = 0.0f; + new_text.color.a = 1.0; + new_text.text = std::to_string(num_wps_); + + rviz_wp_pub_->publish(new_marker); + rviz_wp_pub_->publish(line_list_); + rviz_wp_pub_->publish(new_text); + + ++num_wps_; } -void RvizWaypointPublisher::update_list() { - rclcpp::Time now = this->get_clock()->now(); - line_list_.header.stamp = now; - line_list_.header.frame_id = "NED"; - line_list_.ns = "wp_path"; - line_list_.id = 0; - line_list_.type = visualization_msgs::msg::Marker::LINE_STRIP; - line_list_.action = visualization_msgs::msg::Marker::ADD; - line_list_.scale.x = 3.0; - line_list_.color.r = 0.0f; - line_list_.color.g = 1.0f; - line_list_.color.b = 0.0f; - line_list_.color.a = 1.0; - line_list_.points = line_points_; +void RvizWaypointPublisher::update_list() +{ + rclcpp::Time now = this->get_clock()->now(); + line_list_.header.stamp = now; + line_list_.header.frame_id = "NED"; + line_list_.ns = "wp_path"; + line_list_.id = 0; + line_list_.type = visualization_msgs::msg::Marker::LINE_STRIP; + line_list_.action = visualization_msgs::msg::Marker::ADD; + line_list_.scale.x = 3.0; + line_list_.color.r = 0.0f; + line_list_.color.g = 1.0f; + line_list_.color.b = 0.0f; + line_list_.color.a = 1.0; + line_list_.points = line_points_; } -void RvizWaypointPublisher::update_aircraft_history() { - rclcpp::Time now = this->get_clock()->now(); - aircraft_history_.header.stamp = now; - aircraft_history_.header.frame_id = "NED"; - aircraft_history_.ns = "vehicle_path"; - aircraft_history_.id = 0; - aircraft_history_.type = visualization_msgs::msg::Marker::LINE_STRIP; - aircraft_history_.action = visualization_msgs::msg::Marker::ADD; - aircraft_history_.scale.x = 1.0; - aircraft_history_.scale.y = 1.0; - aircraft_history_.scale.z = 1.0; - aircraft_history_.color.r = 0.0f; - aircraft_history_.color.g = 0.0f; - aircraft_history_.color.b = 0.0f; - aircraft_history_.color.a = 1.0; - aircraft_history_.points = aircraft_history_points_; - - // Restrict length of history - if (aircraft_history_points_.size() > MAX_PATH_HISTORY) { - aircraft_history_points_.erase(aircraft_history_points_.begin()); - } +void RvizWaypointPublisher::update_aircraft_history() +{ + rclcpp::Time now = this->get_clock()->now(); + aircraft_history_.header.stamp = now; + aircraft_history_.header.frame_id = "NED"; + aircraft_history_.ns = "vehicle_path"; + aircraft_history_.id = 0; + aircraft_history_.type = visualization_msgs::msg::Marker::LINE_STRIP; + aircraft_history_.action = visualization_msgs::msg::Marker::ADD; + aircraft_history_.scale.x = 1.0; + aircraft_history_.scale.y = 1.0; + aircraft_history_.scale.z = 1.0; + aircraft_history_.color.r = 0.0f; + aircraft_history_.color.g = 0.0f; + aircraft_history_.color.b = 0.0f; + aircraft_history_.color.a = 1.0; + aircraft_history_.points = aircraft_history_points_; + + // Restrict length of history + if (aircraft_history_points_.size() > MAX_PATH_HISTORY) { + aircraft_history_points_.erase(aircraft_history_points_.begin()); + } } -void RvizWaypointPublisher::update_mesh() { - rclcpp::Time now = this->get_clock()->now(); - aircraft_.header.stamp = now; - - geometry_msgs::msg::TransformStamped t; - t.header.stamp = now; - t.header.frame_id = "NED"; - t.child_frame_id = "aircraft_body"; - t.transform.translation.x = vehicle_state_.position[0]; - t.transform.translation.y = vehicle_state_.position[1]; - t.transform.translation.z = vehicle_state_.position[2]; - - tf2::Quaternion q; - q.setRPY(vehicle_state_.phi, vehicle_state_.theta, vehicle_state_.chi); - t.transform.rotation.x = q.x(); //0.0; //vehicle_state_.quat[0]; - t.transform.rotation.y = q.y(); //0.0; //vehicle_state_.quat[1]; - t.transform.rotation.z = q.z(); //0.0; //vehicle_state_.quat[2]; - t.transform.rotation.w = q.w(); //1.0; //vehicle_state_.quat[3]; - - // Update aircraft history - if (i_ % PATH_PUBLISH_MOD == 0) { - geometry_msgs::msg::Point new_p; - new_p.x = vehicle_state_.position[0]; - new_p.y = vehicle_state_.position[1]; - new_p.z = vehicle_state_.position[2]; - aircraft_history_points_.push_back(new_p); - update_aircraft_history(); - - rviz_aircraft_path_pub_->publish(aircraft_history_); - } - - aircraft_tf2_broadcaster_->sendTransform(t); - rviz_mesh_pub_->publish(aircraft_); +void RvizWaypointPublisher::update_mesh() +{ + rclcpp::Time now = this->get_clock()->now(); + aircraft_.header.stamp = now; + + geometry_msgs::msg::TransformStamped t; + t.header.stamp = now; + t.header.frame_id = "NED"; + t.child_frame_id = "aircraft_body"; + t.transform.translation.x = vehicle_state_.position[0]; + t.transform.translation.y = vehicle_state_.position[1]; + t.transform.translation.z = vehicle_state_.position[2]; + + tf2::Quaternion q; + q.setRPY(vehicle_state_.phi, vehicle_state_.theta, vehicle_state_.chi); + t.transform.rotation.x = q.x(); //0.0; //vehicle_state_.quat[0]; + t.transform.rotation.y = q.y(); //0.0; //vehicle_state_.quat[1]; + t.transform.rotation.z = q.z(); //0.0; //vehicle_state_.quat[2]; + t.transform.rotation.w = q.w(); //1.0; //vehicle_state_.quat[3]; + + // Update aircraft history + if (i_ % PATH_PUBLISH_MOD == 0) { + geometry_msgs::msg::Point new_p; + new_p.x = vehicle_state_.position[0]; + new_p.y = vehicle_state_.position[1]; + new_p.z = vehicle_state_.position[2]; + aircraft_history_points_.push_back(new_p); + update_aircraft_history(); + + rviz_aircraft_path_pub_->publish(aircraft_history_); + } + + aircraft_tf2_broadcaster_->sendTransform(t); + rviz_mesh_pub_->publish(aircraft_); } -void RvizWaypointPublisher::state_update_callback(const rosplane_msgs::msg::State & msg) { - vehicle_state_ = msg; - update_mesh(); +void RvizWaypointPublisher::state_update_callback(const rosplane_msgs::msg::State & msg) +{ + vehicle_state_ = msg; + update_mesh(); } } // namespace rosplane_gcs -int main(int argc, char ** argv) { - rclcpp::init(argc, argv); +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); - rclcpp::spin(node); + rclcpp::spin(node); - return 0; + return 0; } \ No newline at end of file diff --git a/rosplane_tuning/src/signal_generator.cpp b/rosplane_tuning/src/signal_generator.cpp index 9e9d74b..3fc75b2 100644 --- a/rosplane_tuning/src/signal_generator.cpp +++ b/rosplane_tuning/src/signal_generator.cpp @@ -117,10 +117,14 @@ void TuningSignalGenerator::publish_timer_callback() } // Check if step toggle needs to be reset - if (signal_type_ != SignalType::STEP) { step_toggled_ = false; } + if (signal_type_ != SignalType::STEP) { + step_toggled_ = false; + } // If paused, negate passing of time but keep publishing - if (is_paused_) { paused_time_ += 1 / publish_rate_hz_; } + if (is_paused_) { + paused_time_ += 1 / publish_rate_hz_; + } // Get value for signal double amplitude = signal_magnitude_ / 2; @@ -196,7 +200,9 @@ rcl_interfaces::msg::SetParametersResult TuningSignalGenerator::param_callback(const std::vector & params) { for (const auto & param : params) { - if (param.get_name() == "controller_output" || param.get_name() == "signal_type") { reset(); } + if (param.get_name() == "controller_output" || param.get_name() == "signal_type") { + reset(); + } } rcl_interfaces::msg::SetParametersResult result;