From 47eadcae3533dbcbfb5fd08b32792a504dc357b8 Mon Sep 17 00:00:00 2001 From: JMoore5353 Date: Fri, 8 Mar 2024 12:28:23 -0700 Subject: [PATCH] fix code style script --- rosplane/include/controller_base.hpp | 158 ++--- rosplane/include/controller_state_machine.hpp | 24 +- .../include/controller_successive_loop.hpp | 77 +-- rosplane/include/controller_total_energy.hpp | 41 +- rosplane/include/estimator_base.hpp | 48 +- rosplane/include/estimator_example.hpp | 45 +- rosplane/include/path_follower_base.hpp | 38 +- rosplane/include/path_follower_example.hpp | 6 +- rosplane/include/path_manager_base.hpp | 65 +- rosplane/include/path_manager_example.hpp | 49 +- rosplane/src/controller_base.cpp | 239 +++++--- rosplane/src/controller_state_machine.cpp | 15 +- rosplane/src/controller_successive_loop.cpp | 180 +++--- rosplane/src/controller_total_energy.cpp | 58 +- rosplane/src/estimator_base.cpp | 144 ++--- rosplane/src/estimator_example.cpp | 328 +++++----- rosplane/src/path_follower_base.cpp | 94 ++- rosplane/src/path_follower_example.cpp | 49 +- rosplane/src/path_manager_base.cpp | 95 +-- rosplane/src/path_manager_example.cpp | 568 ++++++++---------- rosplane/src/path_planner.cpp | 103 ++-- .../src/gazebo_state_transcription.cpp | 40 +- 22 files changed, 1223 insertions(+), 1241 deletions(-) mode change 100755 => 100644 rosplane/src/path_manager_base.cpp mode change 100755 => 100644 rosplane/src/path_manager_example.cpp diff --git a/rosplane/include/controller_base.hpp b/rosplane/include/controller_base.hpp index 402f42a..5f9e60c 100644 --- a/rosplane/include/controller_base.hpp +++ b/rosplane/include/controller_base.hpp @@ -10,15 +10,15 @@ #ifndef CONTROLLER_BASE_H #define CONTROLLER_BASE_H +#include +#include +#include #include -#include -#include +#include #include #include -#include -#include -#include -#include +#include +#include using std::placeholders::_1; using namespace std::chrono_literals; @@ -30,9 +30,9 @@ namespace rosplane */ enum class alt_zones { - TAKE_OFF, /**< In the take off zone where the aircraft gains speed and altitude */ - CLIMB, /**< In the climb zone the aircraft proceeds to commanded altitude without course change. */ - ALTITUDE_HOLD /**< In the altitude hold zone the aircraft keeps altitude and follows commanded course */ + TAKE_OFF, /**< In the take off zone where the aircraft gains speed and altitude */ + CLIMB, /**< In the climb zone the aircraft proceeds to commanded altitude without course change. */ + ALTITUDE_HOLD /**< In the altitude hold zone the aircraft keeps altitude and follows commanded course */ }; /** @@ -42,32 +42,30 @@ enum class alt_zones class controller_base : public rclcpp::Node { public: - /** * Constructor for ROS2 setup and parameter initialization. */ controller_base(); protected: - /** * This struct holds all of the inputs to the control algorithm. */ struct input_s { - float Ts; /**< time step */ - float h; /**< altitude */ - float va; /**< airspeed */ - float phi; /**< roll angle */ - float theta; /**< pitch angle */ - float chi; /**< course angle */ - float p; /**< body frame roll rate */ - float q; /**< body frame pitch rate */ - float r; /**< body frame yaw rate */ - float Va_c; /**< commanded airspeed (m/s) */ - float h_c; /**< commanded altitude (m) */ - float chi_c; /**< commanded course (rad) */ - float phi_ff; /**< feed forward term for orbits (rad) */ + float Ts; /**< time step */ + float h; /**< altitude */ + float va; /**< airspeed */ + float phi; /**< roll angle */ + float theta; /**< pitch angle */ + float chi; /**< course angle */ + float p; /**< body frame roll rate */ + float q; /**< body frame pitch rate */ + float r; /**< body frame yaw rate */ + float Va_c; /**< commanded airspeed (m/s) */ + float h_c; /**< commanded altitude (m) */ + float chi_c; /**< commanded course (rad) */ + float phi_ff; /**< feed forward term for orbits (rad) */ }; /** @@ -140,21 +138,22 @@ class controller_base : public rclcpp::Node * @param input Inputs to the control algorithm. * @param output Outputs of the controller, including selected intermediate values and final control efforts. */ - virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; + virtual void control(const struct params_s & params, const struct input_s & input, + struct output_s & output) = 0; /** * The override for the intermediate values for the controller. */ - rosplane_msgs::msg::ControllerInternalsDebug tuning_debug_override_msg_; // TODO find a better and more permanent place for this. + rosplane_msgs::msg::ControllerInternalsDebug + tuning_debug_override_msg_; // TODO find a better and more permanent place for this. private: - /** * This publisher publishes the final calculated control surface deflections. */ rclcpp::Publisher::SharedPtr actuators_pub_; - /** + /** * This publisher publishes the intermediate commands in the control algorithm. */ rclcpp::Publisher::SharedPtr internals_pub_; @@ -169,58 +168,60 @@ class controller_base : public rclcpp::Node */ rclcpp::Subscription::SharedPtr vehicle_state_sub_; + /** + * Subscribes to the tuning messages for later use in the control calculations + */ rclcpp::Subscription::SharedPtr tuning_debug_sub_; + /** * This timer controls how often commands are published by the autopilot. */ rclcpp::TimerBase::SharedPtr timer_; /** Parameters for use in control loops.*/ - struct params_s params_ = { - /* alt_hz */ 10.0, - /* alt_toz */ 5.0, - /* tau */ 50.0, - /* c_kp */ 2.37, - /* c_kd */ 0.0, - /* c_ki */ 0.4, - /* r_kp */ .06, - /* r_kd */ 0.04, - /* r_ki */ 0.0, - /* p_kp */ -.15, - /* p_kd */ -.05, - /* p_ki */ 0.0, - /* p_ff */ 0.0, - /* a_t_kp */ .05, - /* a_t_kd */ 0.0, - /* a_t_ki */ .005, - /* a_kp */ 0.015, - /* a_kd */ 0.0, - /* a_ki */ 0.003, - /* l_kp */ 1.0, - /* l_kd */ 0.0, - /* l_ki */ 0.05, - /* e_kp */ 5.0, - /* e_kd */ 0.0, - /* e_ki */ .9, - /* trim_e */ 0.02, - /* trim_a */ 0.0, - /* trim_r */ 0.0, - /* trim_t */ 0.5, - /* max_e */ 0.61, - /* max_a */ 0.15, - /* max_r */ 0.523, - /* max_t */ 1.0, - /* pwm_rad_e */ 1.0, - /* pwm_rad_a */ 1.0, - /* pwm_rad_r */ 1.0, - /* max_takeoff_throttle */ .55, - /* mass */ 2.28, - /* gravity */ 9.8, - /* pitch_tuning_debug_override*/ false, - /* roll_tuning_debug_override*/ false, - /* max_roll*/ 25.0, - /* frequency of contorl loop */ 100 - }; + struct params_s params_ = {/* alt_hz */ 10.0, + /* alt_toz */ 5.0, + /* tau */ 50.0, + /* c_kp */ 2.37, + /* c_kd */ 0.0, + /* c_ki */ 0.4, + /* r_kp */ .06, + /* r_kd */ 0.04, + /* r_ki */ 0.0, + /* p_kp */ -.15, + /* p_kd */ -.05, + /* p_ki */ 0.0, + /* p_ff */ 0.0, + /* a_t_kp */ .05, + /* a_t_kd */ 0.0, + /* a_t_ki */ .005, + /* a_kp */ 0.015, + /* a_kd */ 0.0, + /* a_ki */ 0.003, + /* l_kp */ 1.0, + /* l_kd */ 0.0, + /* l_ki */ 0.05, + /* e_kp */ 5.0, + /* e_kd */ 0.0, + /* e_ki */ .9, + /* trim_e */ 0.02, + /* trim_a */ 0.0, + /* trim_r */ 0.0, + /* trim_t */ 0.5, + /* max_e */ 0.61, + /* max_a */ 0.15, + /* max_r */ 0.523, + /* max_t */ 1.0, + /* pwm_rad_e */ 1.0, + /* pwm_rad_a */ 1.0, + /* pwm_rad_r */ 1.0, + /* max_takeoff_throttle */ .55, + /* mass */ 2.28, + /* gravity */ 9.8, + /* pitch_tuning_debug_override*/ false, + /* roll_tuning_debug_override*/ false, + /* max_roll*/ 25.0, + /* frequency of contorl loop */ 100}; /** * The stored value for the most up to date commands for the controller. @@ -240,7 +241,7 @@ class controller_base : public rclcpp::Node /** * Convert from deflection angle in radians to pwm. */ - void convert_to_pwm(struct output_s &output); + void convert_to_pwm(struct output_s & output); /** * Calls the control function and publishes outputs and intermediate values to the command and controller internals @@ -280,7 +281,8 @@ class controller_base : public rclcpp::Node * @param parameters Set of updated parameters. * @return Service result object that tells the requester the result of the param update. */ - rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector ¶meters); + rcl_interfaces::msg::SetParametersResult + parametersCallback(const std::vector & parameters); /** * This declares each parameter as a parameter so that the ROS2 parameter system can recognize each parameter. @@ -293,9 +295,11 @@ class controller_base : public rclcpp::Node */ void set_parameters(); + /** + * This creates a wall timer that calls the controller publisher. + */ void set_timer(); - }; -} //end namespace +} // namespace rosplane #endif // CONTROLLER_BASE_H diff --git a/rosplane/include/controller_state_machine.hpp b/rosplane/include/controller_state_machine.hpp index fdd874b..97a94ac 100644 --- a/rosplane/include/controller_state_machine.hpp +++ b/rosplane/include/controller_state_machine.hpp @@ -3,14 +3,14 @@ #include "controller_base.hpp" -namespace rosplane{ +namespace rosplane +{ class controller_state_machine : public controller_base { public: - - controller_state_machine(); + controller_state_machine(); /** * The state machine for the control algorithm for the autopilot. @@ -18,10 +18,10 @@ class controller_state_machine : public controller_base * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void control(const struct params_s & params, const struct input_s & input, + struct output_s & output); protected: - /** * The current zone of the control algorithm based on altitude. */ @@ -33,7 +33,8 @@ class controller_state_machine : public controller_base * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; + virtual void take_off(const struct params_s & params, const struct input_s & input, + struct output_s & output) = 0; /** * This function continually loops while the aircraft is in the climb zone. It is implemented by the child. @@ -41,7 +42,8 @@ class controller_state_machine : public controller_base * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; + virtual void climb(const struct params_s & params, const struct input_s & input, + struct output_s & output) = 0; /** * This function continually loops while the aircraft is in the altitude hold zone. It is implemented by the child. @@ -49,7 +51,8 @@ class controller_state_machine : public controller_base * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; + virtual void altitude_hold(const struct params_s & params, const struct input_s & input, + struct output_s & output) = 0; /** * This function runs when the aircraft exits the take-off zone this is often used to reset integrator values. It is @@ -68,11 +71,8 @@ class controller_state_machine : public controller_base * It is implemented by the child. */ virtual void altitude_hold_exit() = 0; - - }; - -} // end namespace +} // namespace rosplane #endif //BUILD_CONTROLLER_STATE_MACHINE_H diff --git a/rosplane/include/controller_successive_loop.hpp b/rosplane/include/controller_successive_loop.hpp index a2f1dfd..6478155 100644 --- a/rosplane/include/controller_successive_loop.hpp +++ b/rosplane/include/controller_successive_loop.hpp @@ -22,8 +22,9 @@ class controller_successive_loop : public controller_state_machine * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); - + virtual void take_off(const struct params_s & params, const struct input_s & input, + struct output_s & output); + /** * This function continually loops while the aircraft is in the climb zone. The lateral and longitudinal control * for the climb zone is called in this function. @@ -31,8 +32,9 @@ class controller_successive_loop : public controller_state_machine * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); - + virtual void climb(const struct params_s & params, const struct input_s & input, + struct output_s & output); + /** * This function continually loops while the aircraft is in the altitude hold zone. The lateral and longitudinal * control for the altitude hold zone is called in this function. @@ -40,59 +42,64 @@ class controller_successive_loop : public controller_state_machine * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); - + virtual void altitude_hold(const struct params_s & params, const struct input_s & input, + struct output_s & output); + /** * This function runs when the aircraft exits the take-off zone. Any changes to the controller that need to happen * only once as the aircraft exits take-off mode should be placed here. This sets differentiators and integrators to 0. */ virtual void take_off_exit(); - + /** * This function runs when the aircraft exits the climb zone. Any changes to the controller that need to happen * only once as the aircraft exits climb mode should be placed here. This sets differentiators and integrators to 0. */ virtual void climb_exit(); - + /** * This function runs when the aircraft exits the altitude hold zone (usually a crash). Any changes to the controller that * need to happen only once as the aircraft exits altitude mode should be placed here. This sets differentiators and * integrators to 0. */ virtual void altitude_hold_exit(); - - + /** * This function runs the lateral control loops for the altitude hold zone. * @param params The parameters that define the algorithm such as control gains. * @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_lateral_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); - + */ + virtual void alt_hold_lateral_control(const struct params_s & params, + const struct input_s & input, struct output_s & output); + /** * This function runs the longitudinal control loops for the altitude hold zone. * @param params The parameters that define the algorithm such as control gains. * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); + */ + virtual void alt_hold_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output); /** * This function runs the lateral control loops for the climb zone. * @param params The parameters that define the algorithm such as control gains. * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); - + */ + virtual void climb_lateral_control(const struct params_s & params, const struct input_s & input, + struct output_s & output); + /** * This function runs the longitudinal control loops for the climb zone. * @param params The parameters that define the algorithm such as control gains. * @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_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void climb_longitudinal_control(const struct params_s & params, + const struct input_s & input, struct output_s & output); /** * This function runs the lateral control loops for the take-off zone. @@ -100,15 +107,18 @@ class controller_successive_loop : public controller_state_machine * @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_lateral_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); - + virtual void take_off_lateral_control(const struct params_s & params, + const struct input_s & input, struct output_s & output); + /** * This function runs the longitudinal control loops for the take-off zone. * @param params The parameters that define the algorithm such as control gains. * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void take_off_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output); /** * The control loop for moving to and holding a commanded course. @@ -119,7 +129,7 @@ class controller_successive_loop : public controller_state_machine * @param params The parameters for the control algorithm, including the control gains. * @return The commanded roll angle, to achieve the course angle. */ - float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s ¶ms); + float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s & params); /** * The difference between the commanded course angle and the current course angle. @@ -139,7 +149,7 @@ class controller_successive_loop : public controller_state_machine * @param params The parameters for the control algorithm, including the control gains and max deflection. * @return The aileron deflection in radians required to achieve the commanded roll angle. */ - float roll_hold(float phi_c, float phi, float p, const struct params_s ¶ms); + float roll_hold(float phi_c, float phi, float p, const struct params_s & params); /** * The difference between the commanded roll angle and the current roll angle. @@ -159,7 +169,7 @@ class controller_successive_loop : public controller_state_machine * @param params The parameters for the control algorithm, including the control gains and max deflection. * @return The elevator deflection in radians required to achieve the commanded pitch. */ - float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms); + float pitch_hold(float theta_c, float theta, float q, const struct params_s & params); /** * The difference between the commanded pitch angle and the current pitch angle. @@ -178,7 +188,7 @@ class controller_successive_loop : public controller_state_machine * @param params The parameters for the control algorithm, including control gains. * @return The required throttle between 0 (no throttle) and 1 (full throttle). */ - float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s ¶ms); + float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s & params); /** * The difference between the commanded airspeed and the current airspeed. @@ -202,7 +212,7 @@ class controller_successive_loop : public controller_state_machine * @param params The parameters for the control algorithm, including control gains. * @return The commanded pitch angle to maintain and achieve the commanded altitude. */ - float altitude_hold_control(float h_c, float h, const struct params_s ¶ms); + float altitude_hold_control(float h_c, float h, const struct params_s & params); /** * The difference between the commanded altitude and the current altitude. @@ -219,12 +229,12 @@ class controller_successive_loop : public controller_state_machine */ float a_differentiator_; -// float cooridinated_turn_hold(float v, const struct params_s ¶ms, float Ts); // TODO implement if you want... -// float ct_error_; -// float ct_integrator_; -// float ct_differentiator_; + // float cooridinated_turn_hold(float v, const struct params_s ¶ms, float Ts); // TODO implement if you want... + // float ct_error_; + // float ct_integrator_; + // float ct_differentiator_; -/** + /** * 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. @@ -235,8 +245,7 @@ class controller_successive_loop : public controller_state_machine float sat(float value, float up_limit, float low_limit); float adjust_h_c(float h_c, float h, float max_diff); - }; -} //end namespace +} // namespace rosplane #endif // CONTROLLER_EXAMPLE_H diff --git a/rosplane/include/controller_total_energy.hpp b/rosplane/include/controller_total_energy.hpp index 22d742d..11a5bc8 100644 --- a/rosplane/include/controller_total_energy.hpp +++ b/rosplane/include/controller_total_energy.hpp @@ -6,24 +6,24 @@ namespace rosplane { - class controller_total_energy : public controller_successive_loop +class controller_total_energy : public controller_successive_loop { - public: +public: /** * Constructor to initialize node. */ controller_total_energy(); - protected: - - +protected: /** * This function overrides the longitudinal control loops for the take-off zone. * @param params The parameters that define the algorithm such as control gains. * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void take_off_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output); /** * This function overrides the longitudinal control loops for the climb zone. @@ -31,7 +31,8 @@ namespace rosplane * @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_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void climb_longitudinal_control(const struct params_s & params, + const struct input_s & input, struct output_s & output); /** * This function overrides the longitudinal control loops for the altitude hold zone. @@ -39,27 +40,29 @@ namespace rosplane * @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 struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void alt_hold_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output); /** * This function overrides when the aircraft exits the take-off zone. Any changes to the controller that need to happen * only once as the aircraft exits take-off mode should be placed here. This sets differentiators and integrators to 0. */ virtual void take_off_exit(); - + /** * This function overrides when the aircraft exits the climb zone. Any changes to the controller that need to happen * only once as the aircraft exits climb mode should be placed here. This sets differentiators and integrators to 0. */ virtual void climb_exit(); - + /** * This function overrides when the aircraft exits the altitude hold zone (usually a crash). Any changes to the controller that * need to happen only once as the aircraft exits altitude mode should be placed here. This sets differentiators and * integrators to 0. */ virtual void altitude_hold_exit(); - + /** * This uses the error in total energy to find the necessary throttle to acheive that energy. * @param va_c This is the commanded airspeed. @@ -70,8 +73,9 @@ namespace rosplane * @param Ts The sampling period in seconds. * @return The throttle value saturated between 0 and the parameter of max throttle. */ - float total_energy_throttle(float va_c, float va, float h_c, float h, const struct params_s ¶ms); - + float total_energy_throttle(float va_c, float va, float h_c, float h, + const struct params_s & params); + /** * This uses the error in the balance of energy to find the necessary elevator deflection to acheive that energy. * @param va_c This is the commanded airspeed. @@ -82,7 +86,8 @@ namespace rosplane * @param Ts The sampling period in seconds. * @return The pitch command value saturated between min and max pitch. */ - float total_energy_pitch(float va_c, float va, float h_c, float h, const struct params_s ¶ms); + float total_energy_pitch(float va_c, float va, float h_c, float h, + const struct params_s & params); /** * This calculates and updates the kinetic energy reference and error, the potential energy error. @@ -92,7 +97,7 @@ namespace rosplane * @param h This is the actual altitude. * @param params The parameters that define the algorithm such as control gains. */ - void update_energies(float va_c, float va, float h_c, float h, const struct params_s ¶ms); + void update_energies(float va_c, float va, float h_c, float h, const struct params_s & params); /** * This is the integral value for the error in the total energy. @@ -108,7 +113,7 @@ namespace rosplane * This is the current reference (desired) kinetic energy. */ float K_ref; - + /** * This is the current error in the kinetic energy. */ @@ -128,9 +133,7 @@ namespace rosplane * The previous error in the total energy. */ float E_error_prev_; - }; -} //end namespace - +} // namespace rosplane #endif //BUILD_CONTROLLER_TOTAL_ENERGY_H diff --git a/rosplane/include/estimator_base.hpp b/rosplane/include/estimator_base.hpp index 2f17c87..53830d5 100644 --- a/rosplane/include/estimator_base.hpp +++ b/rosplane/include/estimator_base.hpp @@ -9,36 +9,33 @@ #ifndef ESTIMATOR_BASE_H #define ESTIMATOR_BASE_H -#include -#include -#include -#include +#include +#include #include -#include -#include -#include #include -#include #include -#include +#include +#include +#include +#include +#include +#include +#include #define EARTH_RADIUS 6378145.0f using std::placeholders::_1; using namespace std::chrono_literals; - namespace rosplane { - class estimator_base : public rclcpp::Node { public: estimator_base(); protected: - struct input_s { float gyro_x; @@ -92,14 +89,16 @@ class estimator_base : public rclcpp::Node double Ts; }; - bool baro_init_; /**< Initial barometric pressure */ + bool baro_init_; /**< Initial barometric pressure */ - virtual void estimate(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; + virtual void estimate(const struct params_s & params, const struct input_s & input, + struct output_s & output) = 0; private: rclcpp::Publisher::SharedPtr vehicle_state_pub_; rclcpp::Subscription::SharedPtr gnss_fix_sub_; - rclcpp::Subscription::SharedPtr gnss_vel_sub_; //used in conjunction with the gnss_fix_sub_ + rclcpp::Subscription::SharedPtr + gnss_vel_sub_; //used in conjunction with the gnss_fix_sub_ rclcpp::Subscription::SharedPtr imu_sub_; rclcpp::Subscription::SharedPtr baro_sub_; rclcpp::Subscription::SharedPtr airspeed_sub_; @@ -124,21 +123,20 @@ class estimator_base : public rclcpp::Node bool gps_new_; 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 */ - bool armed_first_time_; /**< Arm before starting estimation */ - float init_static_; /**< Initial static pressure (mbar) */ - int baro_count_; /**< Used to grab the first set of baro measurements */ + 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 */ + bool armed_first_time_; /**< Arm before starting estimation */ + float init_static_; /**< Initial static pressure (mbar) */ + int baro_count_; /**< Used to grab the first set of baro measurements */ std::vector init_static_vector_; /**< Used to grab the first set of baro measurements */ - struct params_s params_ = {9.8, 1.225, .0245, .01, .01, .005, .005/20., M_PI*.13/180.0, 1.0f/update_rate_}; - - + struct params_s params_ = { + 9.8, 1.225, .0245, .01, .01, .005, .005 / 20., M_PI * .13 / 180.0, 1.0f / update_rate_}; struct input_s input_; }; -} //end namespace +} // namespace rosplane #endif // ESTIMATOR_BASE_H diff --git a/rosplane/include/estimator_example.hpp b/rosplane/include/estimator_example.hpp index ccd91aa..c9f1f27 100644 --- a/rosplane/include/estimator_example.hpp +++ b/rosplane/include/estimator_example.hpp @@ -3,8 +3,8 @@ #include "estimator_base.hpp" -#include #include +#include namespace rosplane { @@ -15,12 +15,12 @@ class estimator_example : public estimator_base estimator_example(); private: - virtual void estimate(const params_s ¶ms, const input_s &input, output_s &output); + virtual void estimate(const params_s & params, const input_s & input, output_s & output); -// float gps_n_old_; -// float gps_e_old_; -// float gps_Vg_old_; -// float gps_course_old_; + // float gps_n_old_; + // float gps_e_old_; + // float gps_Vg_old_; + // float gps_course_old_; double lpf_a_; float alpha_; @@ -42,36 +42,35 @@ class estimator_example : public estimator_base float Vwhat_; float phihat_; float thetahat_; - float psihat_ = 0.0; // TODO link to an inital condiditons param + float psihat_ = 0.0; // TODO link to an inital condiditons param Eigen::Vector2f xhat_a_; // 2 - Eigen::Matrix2f P_a_; // 2x2 + Eigen::Matrix2f P_a_; // 2x2 Eigen::VectorXf xhat_p_; // 7 - Eigen::MatrixXf P_p_; // 7x7 + Eigen::MatrixXf P_p_; // 7x7 - Eigen::Matrix2f Q_a_; // 2x2 + Eigen::Matrix2f Q_a_; // 2x2 Eigen::Matrix3f Q_g_; Eigen::Matrix3f R_accel_; - Eigen::Vector2f f_a_; // 2 - Eigen::Matrix2f A_a_; // 2x2 -// float h_a_; + Eigen::Vector2f f_a_; // 2 + Eigen::Matrix2f A_a_; // 2x2 + // float h_a_; Eigen::Vector3f h_a_; - Eigen::Matrix C_a_; // 2 - Eigen::Vector2f L_a_; // 2 + Eigen::Matrix C_a_; // 2 + Eigen::Vector2f L_a_; // 2 - Eigen::MatrixXf Q_p_; // 7x7 - Eigen::MatrixXf R_p_; // 6x6 - Eigen::VectorXf f_p_; // 7 - Eigen::MatrixXf A_p_; // 7x7 + Eigen::MatrixXf Q_p_; // 7x7 + Eigen::MatrixXf R_p_; // 6x6 + Eigen::VectorXf f_p_; // 7 + Eigen::MatrixXf A_p_; // 7x7 float h_p_; - Eigen::VectorXf C_p_; // 7 - Eigen::VectorXf L_p_; // 7 + Eigen::VectorXf C_p_; // 7 + Eigen::VectorXf L_p_; // 7 float gate_threshold_ = 9.21; // chi2(q = .01, df = 2) void check_xhat_a(); - }; -} //end namespace +} // namespace rosplane #endif // ESTIMATOR_EXAMPLE_H diff --git a/rosplane/include/path_follower_base.hpp b/rosplane/include/path_follower_base.hpp index b60673d..14f9ce9 100644 --- a/rosplane/include/path_follower_base.hpp +++ b/rosplane/include/path_follower_base.hpp @@ -2,9 +2,9 @@ #define PATH_FOLLOWER_BASE_H #include -#include #include #include +#include using namespace std::chrono_literals; using std::placeholders::_1; @@ -25,7 +25,6 @@ class path_follower_base : public rclcpp::Node float spin(); protected: - struct input_s { enum path_type p_type; @@ -35,20 +34,20 @@ class path_follower_base : public rclcpp::Node float c_orbit[3]; float rho_orbit; int lam_orbit; - float pn; /** position north */ - float pe; /** position east */ - float h; /** altitude */ - float Va; /** airspeed */ - float chi; /** course angle */ - float psi; /** heading angle */ + float pn; /** position north */ + float pe; /** position east */ + float h; /** altitude */ + float Va; /** airspeed */ + float chi; /** course angle */ + float psi; /** heading angle */ }; struct output_s { - double Va_c; /** commanded airspeed (m/s) */ - double h_c; /** commanded altitude (m) */ - double chi_c; /** commanded course (rad) */ - double phi_ff; /** feed forward term for orbits (rad) */ + double Va_c; /** commanded airspeed (m/s) */ + double h_c; /** commanded altitude (m) */ + double chi_c; /** commanded course (rad) */ + double phi_ff; /** feed forward term for orbits (rad) */ }; struct params_s @@ -58,11 +57,11 @@ class path_follower_base : public rclcpp::Node double k_orbit; }; - virtual void follow(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; - struct params_s params_ = {.5, 0.05, 4.0}; /**< params */ + virtual void follow(const struct params_s & params, const struct input_s & input, + struct output_s & output) = 0; + struct params_s params_ = {.5, 0.05, 4.0}; /**< params */ private: - rclcpp::Subscription::SharedPtr vehicle_state_sub_; rclcpp::Subscription::SharedPtr current_path_sub_; @@ -78,17 +77,14 @@ class path_follower_base : public rclcpp::Node void current_path_callback(const rosplane_msgs::msg::CurrentPath::SharedPtr msg); bool current_path_init_; - - void update(); - OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - rcl_interfaces::msg::SetParametersResult parametersCallback( - const std::vector ¶meters); + rcl_interfaces::msg::SetParametersResult + parametersCallback(const std::vector & parameters); }; -} // end namespace +} // namespace rosplane #endif // PATH_FOLLOWER_BASE_H diff --git a/rosplane/include/path_follower_example.hpp b/rosplane/include/path_follower_example.hpp index d892465..f3e0660 100644 --- a/rosplane/include/path_follower_example.hpp +++ b/rosplane/include/path_follower_example.hpp @@ -10,9 +10,11 @@ class path_follower_example : public path_follower_base { public: path_follower_example(); + private: - virtual void follow(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void follow(const struct params_s & params, const struct input_s & input, + struct output_s & output); }; -} //end namespace +} // namespace rosplane #endif // PATH_FOLLOWER_EXAMPLE_H diff --git a/rosplane/include/path_manager_base.hpp b/rosplane/include/path_manager_base.hpp index 4f841e4..d375a73 100644 --- a/rosplane/include/path_manager_base.hpp +++ b/rosplane/include/path_manager_base.hpp @@ -10,16 +10,16 @@ #ifndef PATH_MANAGER_BASE_H #define PATH_MANAGER_BASE_H -#include -#include // src/rosplane_msgs/msg/State.msg +#include +#include +#include #include +#include // src/rosplane_msgs/msg/State.msg #include -#include #include +#include #include #include -#include -#include //#include !!! using std::placeholders::_1; using namespace std::chrono_literals; @@ -28,40 +28,39 @@ namespace rosplane { class path_manager_base : public rclcpp::Node { - public: - path_manager_base(); +public: + path_manager_base(); protected: - struct waypoint_s { float w[3]; float chi_d; - bool chi_valid; + bool chi_valid; float va_d; }; std::vector waypoints_; int num_waypoints_; - int idx_a_; /** index to the waypoint that was most recently achieved */ + int idx_a_; /** index to the waypoint that was most recently achieved */ struct input_s { - float pn; /** position north */ - float pe; /** position east */ - float h; /** altitude */ - float chi; /** course angle */ + float pn; /** position north */ + float pe; /** position east */ + float h; /** altitude */ + float chi; /** course angle */ }; struct output_s { - bool flag; /** Inicates strait line or orbital path (true is line, false is orbit) */ - float va_d; /** Desired airspeed (m/s) */ - float r[3]; /** Vector to origin of straight line path (m) */ - float q[3]; /** Unit vector, desired direction of travel for line path */ - float c[3]; /** Center of orbital path (m) */ - float rho; /** Radius of orbital path (m) */ - int8_t lamda; /** Direction of orbital path (cw is 1, ccw is -1) */ + bool flag; /** Inicates strait line or orbital path (true is line, false is orbit) */ + float va_d; /** Desired airspeed (m/s) */ + float r[3]; /** Vector to origin of straight line path (m) */ + float q[3]; /** Unit vector, desired direction of travel for line path */ + float c[3]; /** Center of orbital path (m) */ + float rho; /** Radius of orbital path (m) */ + int8_t lamda; /** Direction of orbital path (cw is 1, ccw is -1) */ }; struct params_s @@ -70,28 +69,32 @@ class path_manager_base : public rclcpp::Node bool orbit_last; }; - virtual void manage(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; + virtual void manage(const struct params_s & params, const struct input_s & input, + struct output_s & output) = 0; private: - - rclcpp::Subscription::SharedPtr vehicle_state_sub_; /**< vehicle state subscription */ - rclcpp::Subscription::SharedPtr new_waypoint_sub_; /**< new waypoint subscription */ - rclcpp::Publisher::SharedPtr current_path_pub_; /**< controller commands publication */ + rclcpp::Subscription::SharedPtr + vehicle_state_sub_; /**< vehicle state subscription */ + rclcpp::Subscription::SharedPtr + new_waypoint_sub_; /**< new waypoint subscription */ + rclcpp::Publisher::SharedPtr + current_path_pub_; /**< controller commands publication */ struct params_s params_; - rosplane_msgs::msg::State vehicle_state_; /**< vehicle state */ + rosplane_msgs::msg::State vehicle_state_; /**< vehicle state */ double update_rate_; rclcpp::TimerBase::SharedPtr update_timer_; - void vehicle_state_callback(const rosplane_msgs::msg::State &msg); + void vehicle_state_callback(const rosplane_msgs::msg::State & msg); bool state_init_; - void new_waypoint_callback(const rosplane_msgs::msg::Waypoint &msg); + void new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg); void current_path_publish(); OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; - rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector ¶meters); + rcl_interfaces::msg::SetParametersResult + parametersCallback(const std::vector & parameters); }; -} //end namespace +} // namespace rosplane #endif // PATH_MANAGER_BASE_H diff --git a/rosplane/include/path_manager_example.hpp b/rosplane/include/path_manager_example.hpp index 53ca041..863b82c 100644 --- a/rosplane/include/path_manager_example.hpp +++ b/rosplane/include/path_manager_example.hpp @@ -5,7 +5,6 @@ #include //#include - #define M_PI_F 3.14159265358979323846f #define M_PI_2_F 1.57079632679489661923f namespace rosplane @@ -30,38 +29,44 @@ class path_manager_example : public path_manager_base { public: path_manager_example(); + private: - virtual void manage(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); + virtual void manage(const struct params_s & params, const struct input_s & input, + struct output_s & output); - void manage_line(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); - void manage_fillet(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); + void manage_line(const struct params_s & params, const struct input_s & input, + struct output_s & output); + void manage_fillet(const struct params_s & params, const struct input_s & input, + struct output_s & output); fillet_state fil_state_; - void manage_dubins(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); + void manage_dubins(const struct params_s & params, const struct input_s & input, + struct output_s & output); dubin_state dub_state_; struct dubinspath_s { - Eigen::Vector3f ps; /** the start position */ - float chis; /** the start course angle */ - Eigen::Vector3f pe; /** the end position */ - float chie; /** the end course angle */ - float R; /** turn radius */ - float L; /** length of the path */ - Eigen::Vector3f cs; /** center of the start circle */ - int lams; /** direction of the start circle */ - Eigen::Vector3f ce; /** center of the endcircle */ - int lame; /** direction of the end circle */ - Eigen::Vector3f w1; /** vector defining half plane H1 */ - Eigen::Vector3f q1; /** unit vector along striaght line path */ - Eigen::Vector3f w2; /** vector defining half plane H2 */ - Eigen::Vector3f w3; /** vector defining half plane H3 */ - Eigen::Vector3f q3; /** unit vector defining direction of half plane H3 */ + Eigen::Vector3f ps; /** the start position */ + float chis; /** the start course angle */ + Eigen::Vector3f pe; /** the end position */ + float chie; /** the end course angle */ + float R; /** turn radius */ + float L; /** length of the path */ + Eigen::Vector3f cs; /** center of the start circle */ + int lams; /** direction of the start circle */ + Eigen::Vector3f ce; /** center of the endcircle */ + int lame; /** direction of the end circle */ + Eigen::Vector3f w1; /** vector defining half plane H1 */ + Eigen::Vector3f q1; /** unit vector along striaght line path */ + Eigen::Vector3f w2; /** vector defining half plane H2 */ + Eigen::Vector3f w3; /** vector defining half plane H3 */ + Eigen::Vector3f q3; /** unit vector defining direction of half plane H3 */ }; struct dubinspath_s dubinspath_; - void dubinsParameters(const struct waypoint_s start_node, const struct waypoint_s end_node, float R); + void dubinsParameters(const struct waypoint_s start_node, const struct waypoint_s end_node, + float R); Eigen::Matrix3f rotz(float theta); float mo(float in); }; -} //end namespace +} // namespace rosplane #endif // PATH_MANAGER_EXAMPLE_H diff --git a/rosplane/src/controller_base.cpp b/rosplane/src/controller_base.cpp index 2bcd9ba..f3bb364 100644 --- a/rosplane/src/controller_base.cpp +++ b/rosplane/src/controller_base.cpp @@ -8,18 +8,20 @@ namespace rosplane { -controller_base::controller_base() : Node("controller_base") +controller_base::controller_base() + : Node("controller_base") { // Advertise published topics. - actuators_pub_ = this->create_publisher("command",10); - internals_pub_ = this->create_publisher("controller_inners_debug",10); + actuators_pub_ = this->create_publisher("command", 10); + internals_pub_ = this->create_publisher( + "controller_inners_debug", 10); // Advertise subscribed topics and set bound callbacks. controller_commands_sub_ = this->create_subscription( - "controller_commands", 10, std::bind(&controller_base::controller_commands_callback, this, _1)); + "controller_commands", 10, std::bind(&controller_base::controller_commands_callback, this, _1)); vehicle_state_sub_ = this->create_subscription( - "estimated_state", 10, std::bind(&controller_base::vehicle_state_callback, this, _1)); + "estimated_state", 10, std::bind(&controller_base::vehicle_state_callback, this, _1)); // This flag indicates whether the first set of commands have been received. command_recieved_ = false; @@ -29,20 +31,21 @@ controller_base::controller_base() : Node("controller_base") // Set the values for the parameters, from the param file or use the deafault value. set_parameters(); - - if (params_.roll_tuning_debug_override || params_.pitch_tuning_debug_override){ + + if (params_.roll_tuning_debug_override || params_.pitch_tuning_debug_override) { tuning_debug_sub_ = this->create_subscription( - "/tuning_debug", 10, std::bind(&controller_base::tuning_debug_callback, this, _1)); + "/tuning_debug", 10, std::bind(&controller_base::tuning_debug_callback, this, _1)); } // Set the parameter callback, for when parameters are changed. parameter_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&controller_base::parametersCallback, this, std::placeholders::_1)); + std::bind(&controller_base::parametersCallback, this, std::placeholders::_1)); set_timer(); } -void controller_base::controller_commands_callback(const rosplane_msgs::msg::ControllerCommands::SharedPtr msg) +void controller_base::controller_commands_callback( + const rosplane_msgs::msg::ControllerCommands::SharedPtr msg) { // Set the flag that a command has been received. @@ -59,7 +62,8 @@ void controller_base::vehicle_state_callback(const rosplane_msgs::msg::State::Sh vehicle_state_ = *msg; } -void controller_base::tuning_debug_callback(const rosplane_msgs::msg::ControllerInternalsDebug::SharedPtr msg) +void controller_base::tuning_debug_callback( + const rosplane_msgs::msg::ControllerInternalsDebug::SharedPtr msg) { // Save the message to use in calculations. tuning_debug_override_msg_ = *msg; @@ -70,7 +74,7 @@ void controller_base::actuator_controls_publish() // Assemble inputs for the control algorithm. struct input_s input; - input.h =-vehicle_state_.position[2]; + input.h = -vehicle_state_.position[2]; input.va = vehicle_state_.va; input.phi = vehicle_state_.phi; input.theta = vehicle_state_.theta; @@ -83,12 +87,10 @@ void controller_base::actuator_controls_publish() input.chi_c = controller_commands_.chi_c; input.phi_ff = controller_commands_.phi_ff; - struct output_s output; // If a command was received, begin control. - if (command_recieved_ == true) - { + if (command_recieved_ == true) { // Control based off of inputs and parameters. control(params_, input, output); @@ -121,27 +123,25 @@ void controller_base::actuator_controls_publish() // If there is a subscriber to the controller inners topic, publish the altitude zone and intermediate // control values, phi_c (commanded roll angle), and theta_c (commanded pitch angle). - if (internals_pub_->get_subscription_count() > 0) - { + if (internals_pub_->get_subscription_count() > 0) { rosplane_msgs::msg::ControllerInternalsDebug inners; inners.header.stamp = now; inners.phi_c = output.phi_c; inners.theta_c = output.theta_c; - switch (output.current_zone) - { - case alt_zones::TAKE_OFF: - inners.alt_zone = inners.ZONE_TAKE_OFF; - break; - case alt_zones::CLIMB: - inners.alt_zone = inners.ZONE_CLIMB; - break; - case alt_zones::ALTITUDE_HOLD: - inners.alt_zone = inners.ZONE_ALTITUDE_HOLD; - break; - default: - break; + switch (output.current_zone) { + case alt_zones::TAKE_OFF: + inners.alt_zone = inners.ZONE_TAKE_OFF; + break; + case alt_zones::CLIMB: + inners.alt_zone = inners.ZONE_CLIMB; + break; + case alt_zones::ALTITUDE_HOLD: + inners.alt_zone = inners.ZONE_ALTITUDE_HOLD; + break; + default: + break; } inners.aux_valid = false; internals_pub_->publish(inners); @@ -149,7 +149,8 @@ void controller_base::actuator_controls_publish() } } -void controller_base::declare_parameters() { +void controller_base::declare_parameters() +{ // Declare each of the parameters, making it visible to the ROS2 param system. this->declare_parameter("alt_hz", params_.alt_hz); @@ -197,7 +198,8 @@ void controller_base::declare_parameters() { this->declare_parameter("frequency", params_.frequency); } -void controller_base::set_parameters() { +void controller_base::set_parameters() +{ // Get the parameters from the launch file, if given. // If not, use the default value defined in the header file. @@ -241,75 +243,117 @@ void controller_base::set_parameters() { params_.mass = this->get_parameter("mass").as_double(); params_.gravity = this->get_parameter("gravity").as_double(); params_.roll_tuning_debug_override = this->get_parameter("roll_tuning_debug_override").as_bool(); - params_.pitch_tuning_debug_override = this->get_parameter("pitch_tuning_debug_override").as_bool(); + params_.pitch_tuning_debug_override = + this->get_parameter("pitch_tuning_debug_override").as_bool(); params_.max_roll = this->get_parameter("max_roll").as_double(); params_.frequency = this->get_parameter("frequency").as_int(); - } -rcl_interfaces::msg::SetParametersResult controller_base::parametersCallback(const std::vector ¶meters) { +rcl_interfaces::msg::SetParametersResult +controller_base::parametersCallback(const std::vector & parameters) +{ rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success"; // Check each parameter in the incoming vector of parameters to change, and change the appropriate parameter. - for (const auto ¶m : parameters) { + for (const auto & param : parameters) { if (param.get_name() == "alt_hz") params_.alt_hz = param.as_double(); - else if (param.get_name() == "alt_toz") params_.alt_toz = param.as_double(); - else if (param.get_name() == "tau") params_.tau = param.as_double(); - else if (param.get_name() == "c_kp") params_.c_kp = param.as_double(); - else if (param.get_name() == "c_kd") params_.c_kd = param.as_double(); - else if (param.get_name() == "c_ki") params_.c_ki = param.as_double(); - else if (param.get_name() == "r_kp") params_.r_kp = param.as_double(); - else if (param.get_name() == "r_kd") params_.r_kd = param.as_double(); - else if (param.get_name() == "r_ki") params_.r_ki = param.as_double(); - else if (param.get_name() == "p_kp") params_.p_kp = param.as_double(); - else if (param.get_name() == "p_kd") params_.p_kd = param.as_double(); - else if (param.get_name() == "p_ki") params_.p_ki = param.as_double(); - else if (param.get_name() == "p_ff") params_.p_ff = param.as_double(); - else if (param.get_name() == "a_t_kp") params_.a_t_kp = param.as_double(); - else if (param.get_name() == "a_t_kd") params_.a_t_kd = param.as_double(); - else if (param.get_name() == "a_t_ki") params_.a_t_ki = param.as_double(); - else if (param.get_name() == "a_kp") params_.a_kp = param.as_double(); - else if (param.get_name() == "a_kd") params_.a_kd = param.as_double(); - else if (param.get_name() == "a_ki") params_.a_ki = param.as_double(); - else if (param.get_name() == "l_kp") params_.l_kp = param.as_double(); - else if (param.get_name() == "l_kd") params_.l_kd = param.as_double(); - else if (param.get_name() == "l_ki") params_.l_ki = param.as_double(); - else if (param.get_name() == "e_kp") params_.e_kp = param.as_double(); - else if (param.get_name() == "e_kd") params_.e_kd = param.as_double(); - else if (param.get_name() == "e_ki") params_.e_ki = param.as_double(); - else if (param.get_name() == "trim_e") params_.trim_e = param.as_double(); - else if (param.get_name() == "trim_a") params_.trim_a = param.as_double(); - else if (param.get_name() == "trim_r") params_.trim_r = param.as_double(); - else if (param.get_name() == "trim_t") params_.trim_t = param.as_double(); - else if (param.get_name() == "max_e") params_.max_e = param.as_double(); - else if (param.get_name() == "max_a") params_.max_a = param.as_double(); - else if (param.get_name() == "max_r") params_.max_r = param.as_double(); - else if (param.get_name() == "max_t") params_.max_t = param.as_double(); - else if (param.get_name() == "pwm_rad_e") params_.pwm_rad_e = param.as_double(); - else if (param.get_name() == "pwm_rad_a") params_.pwm_rad_a = param.as_double(); - else if (param.get_name() == "pwm_rad_r") params_.pwm_rad_r = param.as_double(); - else if (param.get_name() == "max_takeoff_throttle") params_.max_takeoff_throttle = param.as_double(); - else if (param.get_name() == "mass") params_.mass = param.as_double(); - else if (param.get_name() == "gravity") params_.gravity = param.as_double(); - else if (param.get_name() == "roll_tuning_debug_override") params_.roll_tuning_debug_override = param.as_bool(); - else if (param.get_name() == "pitch_tuning_debug_override") params_.pitch_tuning_debug_override = param.as_bool(); - else if (param.get_name() == "max_roll") params_.max_roll = param.as_double(); - else if (param.get_name() == "frequency"){ + else if (param.get_name() == "alt_toz") + params_.alt_toz = param.as_double(); + else if (param.get_name() == "tau") + params_.tau = param.as_double(); + else if (param.get_name() == "c_kp") + params_.c_kp = param.as_double(); + else if (param.get_name() == "c_kd") + params_.c_kd = param.as_double(); + else if (param.get_name() == "c_ki") + params_.c_ki = param.as_double(); + else if (param.get_name() == "r_kp") + params_.r_kp = param.as_double(); + else if (param.get_name() == "r_kd") + params_.r_kd = param.as_double(); + else if (param.get_name() == "r_ki") + params_.r_ki = param.as_double(); + else if (param.get_name() == "p_kp") + params_.p_kp = param.as_double(); + else if (param.get_name() == "p_kd") + params_.p_kd = param.as_double(); + else if (param.get_name() == "p_ki") + params_.p_ki = param.as_double(); + else if (param.get_name() == "p_ff") + params_.p_ff = param.as_double(); + else if (param.get_name() == "a_t_kp") + params_.a_t_kp = param.as_double(); + else if (param.get_name() == "a_t_kd") + params_.a_t_kd = param.as_double(); + else if (param.get_name() == "a_t_ki") + params_.a_t_ki = param.as_double(); + else if (param.get_name() == "a_kp") + params_.a_kp = param.as_double(); + else if (param.get_name() == "a_kd") + params_.a_kd = param.as_double(); + else if (param.get_name() == "a_ki") + params_.a_ki = param.as_double(); + else if (param.get_name() == "l_kp") + params_.l_kp = param.as_double(); + else if (param.get_name() == "l_kd") + params_.l_kd = param.as_double(); + else if (param.get_name() == "l_ki") + params_.l_ki = param.as_double(); + else if (param.get_name() == "e_kp") + params_.e_kp = param.as_double(); + else if (param.get_name() == "e_kd") + params_.e_kd = param.as_double(); + else if (param.get_name() == "e_ki") + params_.e_ki = param.as_double(); + else if (param.get_name() == "trim_e") + params_.trim_e = param.as_double(); + else if (param.get_name() == "trim_a") + params_.trim_a = param.as_double(); + else if (param.get_name() == "trim_r") + params_.trim_r = param.as_double(); + else if (param.get_name() == "trim_t") + params_.trim_t = param.as_double(); + else if (param.get_name() == "max_e") + params_.max_e = param.as_double(); + else if (param.get_name() == "max_a") + params_.max_a = param.as_double(); + else if (param.get_name() == "max_r") + params_.max_r = param.as_double(); + else if (param.get_name() == "max_t") + params_.max_t = param.as_double(); + else if (param.get_name() == "pwm_rad_e") + params_.pwm_rad_e = param.as_double(); + else if (param.get_name() == "pwm_rad_a") + params_.pwm_rad_a = param.as_double(); + else if (param.get_name() == "pwm_rad_r") + params_.pwm_rad_r = param.as_double(); + else if (param.get_name() == "max_takeoff_throttle") + params_.max_takeoff_throttle = param.as_double(); + else if (param.get_name() == "mass") + params_.mass = param.as_double(); + else if (param.get_name() == "gravity") + params_.gravity = param.as_double(); + else if (param.get_name() == "roll_tuning_debug_override") + params_.roll_tuning_debug_override = param.as_bool(); + else if (param.get_name() == "pitch_tuning_debug_override") + params_.pitch_tuning_debug_override = param.as_bool(); + else if (param.get_name() == "max_roll") + params_.max_roll = param.as_double(); + else if (param.get_name() == "frequency") { params_.frequency = param.as_int(); timer_->cancel(); set_timer(); - } - else{ + } else { // If the parameter given doesn't match any of the parameters return false. 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."; break; } - } return result; @@ -317,24 +361,29 @@ rcl_interfaces::msg::SetParametersResult controller_base::parametersCallback(con void controller_base::set_timer() { - auto timer_period = std::chrono::microseconds(static_cast(1.0/params_.frequency * 1'000'000)); + auto timer_period = + std::chrono::microseconds(static_cast(1.0 / params_.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(&controller_base::actuator_controls_publish, this)); // TODO add the period to the params. - + timer_ = this->create_wall_timer(timer_period, + std::bind(&controller_base::actuator_controls_publish, + this)); // TODO add the period to the params. } -void controller_base::convert_to_pwm(controller_base::output_s &output) { +void controller_base::convert_to_pwm(controller_base::output_s & output) +{ // Multiply each control effort (in radians) by a scaling factor to a pwm. - output.delta_e = output.delta_e*params_.pwm_rad_e; // TODO investigate why this is named "pwm". The actual scaling to pwm happens in rosflight_io. - output.delta_a = output.delta_a*params_.pwm_rad_a; - output.delta_r = output.delta_r*params_.pwm_rad_r; + output.delta_e = output.delta_e + * params_ + .pwm_rad_e; // TODO investigate why this is named "pwm". The actual scaling to pwm happens in rosflight_io. + output.delta_a = output.delta_a * params_.pwm_rad_a; + output.delta_r = output.delta_r * params_.pwm_rad_r; } -} //end namespace +} // namespace rosplane -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { // Initialize ROS2 and then begin to spin control node. @@ -342,17 +391,15 @@ int main(int argc, char *argv[]) std::cout << argv[1] << std::endl; - if (strcmp(argv[1],"total_energy") == 0){ + if (strcmp(argv[1], "total_energy") == 0) { auto node = std::make_shared(); RCLCPP_INFO_STREAM(node->get_logger(), "Using total energy control."); rclcpp::spin(node); - } - else if(strcmp(argv[1], "default") == 0){ + } else if (strcmp(argv[1], "default") == 0) { auto node = std::make_shared(); RCLCPP_INFO_STREAM(node->get_logger(), "Using default control."); rclcpp::spin(node); - } - else{ + } else { auto node = std::make_shared(); RCLCPP_INFO_STREAM(node->get_logger(), "Invalid control type, using default control."); rclcpp::spin(node); diff --git a/rosplane/src/controller_state_machine.cpp b/rosplane/src/controller_state_machine.cpp index 1a58c27..a5440cf 100644 --- a/rosplane/src/controller_state_machine.cpp +++ b/rosplane/src/controller_state_machine.cpp @@ -1,16 +1,20 @@ #include "controller_state_machine.hpp" -namespace rosplane { +namespace rosplane +{ -controller_state_machine::controller_state_machine() : controller_base() { +controller_state_machine::controller_state_machine() + : controller_base() +{ // Initialize controller in take_off zone. current_zone = alt_zones::TAKE_OFF; - } -void controller_state_machine::control(const params_s ¶ms, const input_s &input, output_s &output) { +void controller_state_machine::control(const params_s & params, const input_s & input, + output_s & output) +{ // This state machine changes the controls used based on the zone of flight path the aircraft is currently on. switch (current_zone) { @@ -69,7 +73,6 @@ void controller_state_machine::control(const params_s ¶ms, const input_s &in // Set the control zone back to take off to regain altitude. and reset integral for course. RCLCPP_INFO(this->get_logger(), "take off"); current_zone = alt_zones::TAKE_OFF; - } break; default: @@ -80,4 +83,4 @@ void controller_state_machine::control(const params_s ¶ms, const input_s &in output.current_zone = current_zone; } -} // end namespace \ No newline at end of file +} // namespace rosplane \ No newline at end of file diff --git a/rosplane/src/controller_successive_loop.cpp b/rosplane/src/controller_successive_loop.cpp index 3bdf495..a4a19ae 100644 --- a/rosplane/src/controller_successive_loop.cpp +++ b/rosplane/src/controller_successive_loop.cpp @@ -6,13 +6,14 @@ namespace rosplane { - -double wrap_within_180(double fixed_heading, double wrapped_heading) { - // wrapped_heading - number_of_times_to_wrap * 2pi - return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; +double wrap_within_180(double fixed_heading, double wrapped_heading) +{ + // wrapped_heading - number_of_times_to_wrap * 2pi + return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; } -controller_successive_loop::controller_successive_loop() : controller_state_machine() +controller_successive_loop::controller_successive_loop() + : controller_state_machine() { // Initialize course hold, roll hold and pitch hold errors and integrators to zero. c_error_ = 0; @@ -23,7 +24,8 @@ controller_successive_loop::controller_successive_loop() : controller_state_mach p_integrator_ = 0; } -void controller_successive_loop::take_off(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::take_off(const struct params_s & params, + const struct input_s & input, struct output_s & output) { // Run lateral and longitudinal controls. take_off_lateral_control(params, input, output); @@ -35,7 +37,8 @@ void controller_successive_loop::take_off_exit() // Put any code that should run as the airplane exits take off mode. } -void controller_successive_loop::climb(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::climb(const struct params_s & params, const struct input_s & input, + struct output_s & output) { // Run lateral and longitudinal controls. climb_lateral_control(params, input, output); @@ -53,7 +56,9 @@ void controller_successive_loop::climb_exit() a_differentiator_ = 0; } -void controller_successive_loop::altitude_hold(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::altitude_hold(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Run lateral and longitudinal controls. alt_hold_lateral_control(params, input, output); @@ -66,7 +71,9 @@ void controller_successive_loop::altitude_hold_exit() c_integrator_ = 0; } -void controller_successive_loop::alt_hold_lateral_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::alt_hold_lateral_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Set rudder command to zero, can use cooridinated_turn_hold if implemented. // Find commanded roll angle in order to achieve commanded course. @@ -74,14 +81,14 @@ void controller_successive_loop::alt_hold_lateral_control(const struct params_s output.delta_r = 0; //cooridinated_turn_hold(input.beta, params) output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params); - if (params.roll_tuning_debug_override){ - output.phi_c = tuning_debug_override_msg_.phi_c; - } + if (params.roll_tuning_debug_override) { output.phi_c = tuning_debug_override_msg_.phi_c; } output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params); } -void controller_successive_loop::alt_hold_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::alt_hold_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Saturate the altitude command. double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz); @@ -90,14 +97,14 @@ void controller_successive_loop::alt_hold_longitudinal_control(const struct para output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params); output.theta_c = altitude_hold_control(adjusted_hc, input.h, params); - if (params.pitch_tuning_debug_override){ - output.theta_c = tuning_debug_override_msg_.theta_c; - } + if (params.pitch_tuning_debug_override) { output.theta_c = tuning_debug_override_msg_.theta_c; } output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params); } -void controller_successive_loop::climb_lateral_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::climb_lateral_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Maintain straight flight while gaining altitude. output.phi_c = 0; @@ -105,7 +112,9 @@ void controller_successive_loop::climb_lateral_control(const struct params_s &pa output.delta_r = 0; } -void controller_successive_loop::climb_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::climb_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Saturate the altitude command. double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz); @@ -116,7 +125,9 @@ void controller_successive_loop::climb_longitudinal_control(const struct params_ output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params); } -void controller_successive_loop::take_off_lateral_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::take_off_lateral_control(const struct params_s & params, + const struct input_s & input, + struct output_s & 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; @@ -124,10 +135,13 @@ void controller_successive_loop::take_off_lateral_control(const struct params_s output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params); } -void controller_successive_loop::take_off_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_successive_loop::take_off_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Set throttle to not overshoot altitude. - output.delta_t = sat(airspeed_with_throttle_hold(input.Va_c, input.va, params), params.max_takeoff_throttle, 0); + output.delta_t = + sat(airspeed_with_throttle_hold(input.Va_c, input.va, params), params.max_takeoff_throttle, 0); // Command a shallow pitch angle to gain altitude. output.theta_c = 5.0 * 3.14 / 180.0; // TODO add to params. @@ -173,135 +187,130 @@ void controller_successive_loop::take_off_longitudinal_control(const struct para } */ -float controller_successive_loop::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s ¶ms) +float controller_successive_loop::course_hold(float chi_c, float chi, float phi_ff, float r, + const params_s & params) { double wrapped_chi_c = wrap_within_180(chi, chi_c); - float error = wrapped_chi_c - chi; - float Ts = 1.0/params.frequency; + float Ts = 1.0 / params.frequency; - c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_); + c_integrator_ = c_integrator_ + (Ts / 2.0) * (error + c_error_); - float up = params.c_kp*error; - float ui = params.c_ki*c_integrator_; - float ud = params.c_kd*r; + float up = params.c_kp * error; + float ui = params.c_ki * c_integrator_; + float ud = params.c_kd * r; - float phi_c = sat(up + ui + ud + phi_ff, params.max_roll*3.14/180.0, -params.max_roll*3.14/180.0); - if (fabs(params.c_ki) >= 0.00001) - { + float phi_c = + sat(up + ui + ud + phi_ff, params.max_roll * 3.14 / 180.0, -params.max_roll * 3.14 / 180.0); + if (fabs(params.c_ki) >= 0.00001) { float phi_c_unsat = up + ui + ud + phi_ff; - c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat); + c_integrator_ = c_integrator_ + (Ts / params.c_ki) * (phi_c - phi_c_unsat); } c_error_ = error; return phi_c; } -float controller_successive_loop::roll_hold(float phi_c, float phi, float p, const params_s ¶ms) +float controller_successive_loop::roll_hold(float phi_c, float phi, float p, + const params_s & params) { float error = phi_c - phi; - float Ts = 1.0/params.frequency; + float Ts = 1.0 / params.frequency; - r_integrator = r_integrator + (Ts/2.0)*(error + r_error_); + r_integrator = r_integrator + (Ts / 2.0) * (error + r_error_); - float up = params.r_kp*error; - float ui = params.r_ki*r_integrator; - float ud = params.r_kd*p; + float up = params.r_kp * error; + float ui = params.r_ki * r_integrator; + float ud = params.r_kd * p; float delta_a = sat(up + ui - ud, params.max_a, -params.max_a); - if (fabs(params.r_ki) >= 0.00001) - { + if (fabs(params.r_ki) >= 0.00001) { float delta_a_unsat = up + ui - ud; - r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat); + r_integrator = r_integrator + (Ts / params.r_ki) * (delta_a - delta_a_unsat); } r_error_ = error; return delta_a; } -float controller_successive_loop::pitch_hold(float theta_c, float theta, float q, const params_s ¶ms) +float controller_successive_loop::pitch_hold(float theta_c, float theta, float q, + const params_s & params) { float error = theta_c - theta; - float Ts = 1.0/params.frequency; - - p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_); + float Ts = 1.0 / params.frequency; - float up = params.p_kp*error; - float ui = params.p_ki*p_integrator_; - float ud = params.p_kd*q; + p_integrator_ = p_integrator_ + (Ts / 2.0) * (error + p_error_); + float up = params.p_kp * error; + float ui = params.p_ki * p_integrator_; + float ud = params.p_kd * q; - float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui - ud, params.max_e, -params.max_e); + float delta_e = sat(params.trim_e / params.pwm_rad_e + up + ui - ud, params.max_e, -params.max_e); - - if (fabs(params.p_ki) >= 0.00001) - { - float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui - ud; - p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat); + if (fabs(params.p_ki) >= 0.00001) { + float delta_e_unsat = params.trim_e / params.pwm_rad_e + up + ui - ud; + p_integrator_ = p_integrator_ + (Ts / params.p_ki) * (delta_e - delta_e_unsat); } p_error_ = error; return -delta_e; // TODO explain subtraction. } -float controller_successive_loop::airspeed_with_throttle_hold(float Va_c, float Va, const params_s ¶ms) +float controller_successive_loop::airspeed_with_throttle_hold(float Va_c, float Va, + const params_s & params) { float error = Va_c - Va; - float Ts = 1.0/params.frequency; + float Ts = 1.0 / params.frequency; - at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_); - at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 / - (2.0*params.tau + Ts))*(error - at_error_); + at_integrator_ = at_integrator_ + (Ts / 2.0) * (error + at_error_); + at_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * at_differentiator_ + + (2.0 / (2.0 * params.tau + Ts)) * (error - at_error_); - float up = params.a_t_kp*error; - float ui = params.a_t_ki*at_integrator_; - float ud = params.a_t_kd*at_differentiator_; + float up = params.a_t_kp * error; + float ui = params.a_t_ki * at_integrator_; + float ud = params.a_t_kd * at_differentiator_; float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0); - if (fabs(params.a_t_ki) >= 0.00001) - { + if (fabs(params.a_t_ki) >= 0.00001) { float delta_t_unsat = params.trim_t + up + ui + ud; - at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat); + at_integrator_ = at_integrator_ + (Ts / params.a_t_ki) * (delta_t - delta_t_unsat); } at_error_ = error; return delta_t; } -float controller_successive_loop::altitude_hold_control(float h_c, float h, const params_s ¶ms) +float controller_successive_loop::altitude_hold_control(float h_c, float h, const params_s & params) { float error = h_c - h; - float Ts = 1.0/params.frequency; + float Ts = 1.0 / params.frequency; if (-params.alt_hz + .01 < error && error < params.alt_hz - .01) { a_integrator_ = a_integrator_ + (Ts / 2.0) * (error + a_error_); - } - else{ + } else { a_integrator_ = 0.0; } - a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 / - (2.0*params.tau + Ts))*(error - a_error_); + a_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * a_differentiator_ + + (2.0 / (2.0 * params.tau + Ts)) * (error - a_error_); - float up = params.a_kp*error; - float ui = params.a_ki*a_integrator_; - float ud = params.a_kd*a_differentiator_; + float up = params.a_kp * error; + float ui = params.a_ki * a_integrator_; + float ud = params.a_kd * a_differentiator_; - float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -20.0*3.14/180.0); - if (fabs(params.a_ki) >= 0.00001) - { + float theta_c = sat(up + ui + ud, 20.0 * 3.14 / 180.0, -20.0 * 3.14 / 180.0); + if (fabs(params.a_ki) >= 0.00001) { float theta_c_unsat = up + ui + ud; - a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat); + a_integrator_ = a_integrator_ + (Ts / params.a_ki) * (theta_c - theta_c_unsat); } - a_error_ = error; return theta_c; } @@ -318,8 +327,7 @@ float controller_successive_loop::sat(float value, float up_limit, float low_lim // Set to lower limit if smaller than that limit. // Otherwise, do not change the value. 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 @@ -329,19 +337,19 @@ float controller_successive_loop::sat(float value, float up_limit, float low_lim return rVal; } -float controller_successive_loop::adjust_h_c(float h_c, float h, float max_diff) { +float controller_successive_loop::adjust_h_c(float h_c, float h, float max_diff) +{ double adjusted_h_c; // If the error in altitude is larger than the max altitude, adjust it to the max with the correct sign. // Otherwise, proceed as normal. - if (abs(h_c - h) > max_diff){ + if (abs(h_c - h) > max_diff) { adjusted_h_c = h + copysign(max_diff, h_c - h); - } - else{ + } else { adjusted_h_c = h_c; } return adjusted_h_c; } -} //end namespace +} // namespace rosplane diff --git a/rosplane/src/controller_total_energy.cpp b/rosplane/src/controller_total_energy.cpp index 6c02072..34c6e0f 100644 --- a/rosplane/src/controller_total_energy.cpp +++ b/rosplane/src/controller_total_energy.cpp @@ -3,17 +3,21 @@ namespace rosplane { -controller_total_energy::controller_total_energy() : controller_successive_loop() +controller_total_energy::controller_total_energy() + : controller_successive_loop() { // Initialize course hold, roll hold and pitch hold errors and integrators to zero. L_integrator_ = 0; E_integrator_ = 0; } -void controller_total_energy::take_off_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_total_energy::take_off_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Set throttle to not overshoot altitude. - output.delta_t = sat(total_energy_throttle(input.Va_c, input.va, input.h_c, input.h, params), params.max_takeoff_throttle, 0); + output.delta_t = sat(total_energy_throttle(input.Va_c, input.va, input.h_c, input.h, params), + params.max_takeoff_throttle, 0); // Command a shallow pitch angle to gain altitude. output.theta_c = 5.0 * 3.14 / 180.0; //TODO add to params. @@ -24,16 +28,18 @@ void controller_total_energy::take_off_exit() { // Run parent exit code. controller_successive_loop::take_off_exit(); - + L_integrator_ = 0; E_integrator_ = 0; // Place any controller code that should run as you exit the take-off regime here. } -void controller_total_energy::climb_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_total_energy::climb_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { - double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz/2.0); + double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz / 2.0); // Find the control efforts for throttle and find the commanded pitch angle using total energy. output.delta_t = total_energy_throttle(input.Va_c, input.va, adjusted_hc, input.h, params); @@ -45,21 +51,24 @@ void controller_total_energy::climb_exit() { //Run parent exit code controller_successive_loop::climb_exit(); - + L_integrator_ = 0; E_integrator_ = 0; // Place any controller code that should run as you exit the climb regime here. } -void controller_total_energy::alt_hold_longitudinal_control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) +void controller_total_energy::alt_hold_longitudinal_control(const struct params_s & params, + const struct input_s & input, + struct output_s & output) { // Saturate altitude command. double adjusted_hc = adjust_h_c(input.h_c, input.h, params.alt_hz); // Calculate the control effort to maintain airspeed and the required pitch angle to maintain altitude. output.delta_t = total_energy_throttle(input.Va_c, input.va, adjusted_hc, input.h, params); - output.theta_c = total_energy_pitch(input.Va_c, input.va, adjusted_hc, input.h, params); // TODO remove capital from Va_c + output.theta_c = total_energy_pitch(input.Va_c, input.va, adjusted_hc, input.h, + params); // TODO remove capital from Va_c output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params); } @@ -74,7 +83,7 @@ void controller_total_energy::altitude_hold_exit() } float controller_total_energy::total_energy_throttle(float va_c, float va, float h_c, float h, - const struct params_s ¶ms) + const struct params_s & params) { // Update energies based off of most recent data. update_energies(va_c, va, h_c, h, params); @@ -82,23 +91,22 @@ float controller_total_energy::total_energy_throttle(float va_c, float va, float // Calculate total energy error, and normalize relative to the desired kinetic energy. float E_error = (K_error + U_error) / K_ref; - float Ts = 1.0/params.frequency; + float Ts = 1.0 / params.frequency; // Integrate error. - E_integrator_ = E_integrator_ + (Ts/2.0)*(E_error + E_error_prev_); + E_integrator_ = E_integrator_ + (Ts / 2.0) * (E_error + E_error_prev_); E_error_prev_ = E_error; - if (h < .5){ - E_integrator_ = 0; - } + if (h < .5) { E_integrator_ = 0; } // Return saturated throttle command. - return sat(params.e_kp * E_error + params.e_ki * E_integrator_, params.max_t, 0.0) + params.trim_t; + return sat(params.e_kp * E_error + params.e_ki * E_integrator_, params.max_t, 0.0) + + params.trim_t; } float controller_total_energy::total_energy_pitch(float va_c, float va, float h_c, float h, - const struct params_s ¶ms) + const struct params_s & params) { // Update energies based off of most recent data. update_energies(va_c, va, h_c, h, params); @@ -107,28 +115,30 @@ float controller_total_energy::total_energy_pitch(float va_c, float va, float h_ float L_error = (U_error - K_error) / K_ref; - float Ts = 1.0/params.frequency; + float Ts = 1.0 / params.frequency; // Integrate error. - L_integrator_ = L_integrator_ + (Ts/2.0)*(L_error + L_error_prev_); + L_integrator_ = L_integrator_ + (Ts / 2.0) * (L_error + L_error_prev_); L_error_prev_ = L_error; // Return saturated pitch command. - return sat(params.l_kp * L_error + params.l_ki * L_integrator_, 25.0 * M_PI/180.0, -25.0 * M_PI/180.0); // TODO remove hard coded bounds from all of rosplane!!!! + return sat(params.l_kp * L_error + params.l_ki * L_integrator_, 25.0 * M_PI / 180.0, + -25.0 * M_PI / 180.0); // TODO remove hard coded bounds from all of rosplane!!!! } -void controller_total_energy::update_energies(float va_c, float va, float h_c, float h, const struct params_s ¶ms) +void controller_total_energy::update_energies(float va_c, float va, float h_c, float h, + const struct params_s & params) { // Calculate the error in kinetic energy. - K_error = 0.5 * params.mass * (pow(va_c,2) - pow(va,2)); + K_error = 0.5 * params.mass * (pow(va_c, 2) - pow(va, 2)); // Clacualte the desired kinetic energy. - K_ref = 0.5 * params.mass * pow(va_c,2); + K_ref = 0.5 * params.mass * pow(va_c, 2); // Calculate the error in the potential energy. U_error = params.mass * params.gravity * sat(h_c - h, 5, -5); // TODO add limits to param } -} //end namespace +} // namespace rosplane diff --git a/rosplane/src/estimator_base.cpp b/rosplane/src/estimator_base.cpp index 794e8ab..b9742e3 100644 --- a/rosplane/src/estimator_base.cpp +++ b/rosplane/src/estimator_base.cpp @@ -5,16 +5,24 @@ namespace rosplane { -estimator_base::estimator_base() : Node("estimator_base") { +estimator_base::estimator_base() + : Node("estimator_base") +{ vehicle_state_pub_ = this->create_publisher("estimated_state", 10); - gnss_fix_sub_ = this->create_subscription(gnss_fix_topic_, 10, std::bind(&estimator_base::gnssFixCallback, this, std::placeholders::_1)); - gnss_vel_sub_ = this->create_subscription(gnss_vel_topic_, 10, std::bind(&estimator_base::gnssVelCallback, this, std::placeholders::_1)); - imu_sub_ = this->create_subscription(imu_topic_, 10, std::bind(&estimator_base::imuCallback, this, std::placeholders::_1)); - baro_sub_ = this->create_subscription(baro_topic_, 10, std::bind(&estimator_base::baroAltCallback, this, std::placeholders::_1)); - airspeed_sub_ = this->create_subscription(airspeed_topic_, 10, std::bind(&estimator_base::airspeedCallback, this, std::placeholders::_1)); - status_sub_ = this->create_subscription(status_topic_, 10, std::bind(&estimator_base::statusCallback, this, std::placeholders::_1)); + gnss_fix_sub_ = this->create_subscription( + gnss_fix_topic_, 10, std::bind(&estimator_base::gnssFixCallback, this, std::placeholders::_1)); + gnss_vel_sub_ = this->create_subscription( + gnss_vel_topic_, 10, std::bind(&estimator_base::gnssVelCallback, this, std::placeholders::_1)); + imu_sub_ = this->create_subscription( + imu_topic_, 10, std::bind(&estimator_base::imuCallback, this, std::placeholders::_1)); + baro_sub_ = this->create_subscription( + baro_topic_, 10, std::bind(&estimator_base::baroAltCallback, this, std::placeholders::_1)); + airspeed_sub_ = this->create_subscription( + airspeed_topic_, 10, std::bind(&estimator_base::airspeedCallback, this, std::placeholders::_1)); + status_sub_ = this->create_subscription( + status_topic_, 10, std::bind(&estimator_base::statusCallback, this, std::placeholders::_1)); update_timer_ = this->create_wall_timer(10ms, std::bind(&estimator_base::update, this)); @@ -25,10 +33,9 @@ estimator_base::estimator_base() : Node("estimator_base") { params_.sigma_n_gps = .01; params_.sigma_e_gps = .01; params_.sigma_Vg_gps = .005; - params_.sigma_course_gps = .005/20; - - params_.sigma_accel = .0025*9.81; + params_.sigma_course_gps = .005 / 20; + params_.sigma_accel = .0025 * 9.81; } // TODO add param callback. @@ -37,12 +44,9 @@ void estimator_base::update() { struct output_s output; - if (armed_first_time_) - { + if (armed_first_time_) { estimate(params_, input_, output); - } - else - { + } else { output.pn = output.pe = output.h = 0; output.phi = output.theta = output.psi = 0; output.alpha = output.beta = output.chi = 0; @@ -54,13 +58,13 @@ void estimator_base::update() rosplane_msgs::msg::State msg; msg.header.stamp = this->get_clock()->now(); - msg.header.frame_id = 1; // Denotes global frame TODO make sure all messages throughout ROSPlane have this + msg.header.frame_id = + 1; // Denotes global frame TODO make sure all messages throughout ROSPlane have this - msg.position[0] = - output.pn; - msg.position[1] = - output.pe; // TODO find out why there are these minuses. - msg.position[2] = - output.h; - if (gps_init_) - { + msg.position[0] = -output.pn; + msg.position[1] = -output.pe; // TODO find out why there are these minuses. + msg.position[2] = -output.h; + if (gps_init_) { msg.initial_lat = init_lat_; msg.initial_lon = init_lon_; msg.initial_alt = init_alt_; @@ -82,8 +86,8 @@ void estimator_base::update() Eigen::Quaternionf q; q = Eigen::AngleAxisf(output.phi, Eigen::Vector3f::UnitX()) - * Eigen::AngleAxisf(output.theta, Eigen::Vector3f::UnitY()) - * Eigen::AngleAxisf(output.psi, Eigen::Vector3f::UnitZ()); + * Eigen::AngleAxisf(output.theta, Eigen::Vector3f::UnitY()) + * Eigen::AngleAxisf(output.psi, Eigen::Vector3f::UnitZ()); msg.quat[0] = q.w(); msg.quat[1] = q.x(); @@ -94,42 +98,38 @@ void estimator_base::update() msg.v = 0; msg.w = output.Va * sin(output.theta); - msg.psi_deg = fmod(output.psi, 2.0*M_PI)*180/M_PI; //-360 to 360 + msg.psi_deg = fmod(output.psi, 2.0 * M_PI) * 180 / M_PI; //-360 to 360 msg.psi_deg += (msg.psi_deg < -180 ? 360 : 0); msg.psi_deg -= (msg.psi_deg > 180 ? 360 : 0); - msg.chi_deg = fmod(output.chi, 2.0*M_PI)*180/M_PI; //-360 to 360 + msg.chi_deg = fmod(output.chi, 2.0 * M_PI) * 180 / M_PI; //-360 to 360 msg.chi_deg += (msg.chi_deg < -180 ? 360 : 0); msg.chi_deg -= (msg.chi_deg > 180 ? 360 : 0); vehicle_state_pub_->publish(msg); } - void estimator_base::gnssFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg) { - bool has_fix = msg->status.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; // Higher values refer to augmented fixes - if (!has_fix || !std::isfinite(msg->latitude)) - { + bool has_fix = msg->status.status + >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; // Higher values refer to augmented fixes + if (!has_fix || !std::isfinite(msg->latitude)) { input_.gps_new = false; return; } - if (!gps_init_ && has_fix) - { + if (!gps_init_ && has_fix) { RCLCPP_INFO_STREAM(this->get_logger(), "init_lat: " << msg->latitude); gps_init_ = true; init_alt_ = msg->altitude; init_lat_ = msg->latitude; init_lon_ = msg->longitude; - } - else - { - input_.gps_n = EARTH_RADIUS*(msg->latitude - init_lat_)*M_PI/180.0; - input_.gps_e = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg->longitude - init_lon_)*M_PI/180.0; + } else { + input_.gps_n = EARTH_RADIUS * (msg->latitude - init_lat_) * M_PI / 180.0; + input_.gps_e = + EARTH_RADIUS * cos(init_lat_ * M_PI / 180.0) * (msg->longitude - init_lon_) * M_PI / 180.0; input_.gps_h = msg->altitude - init_alt_; input_.gps_new = true; -// RCLCPP_INFO_STREAM(this->get_logger(), "gps_n: " << input_.gps_n); - + // RCLCPP_INFO_STREAM(this->get_logger(), "gps_n: " << input_.gps_n); } } @@ -137,11 +137,12 @@ void estimator_base::gnssVelCallback(const geometry_msgs::msg::TwistStamped::Sha { double v_n = msg->twist.linear.x; double v_e = msg->twist.linear.y; -// double v_d = msg->twist.linear.z; // This variable was unused. + // double v_d = msg->twist.linear.z; // This variable was unused. double ground_speed = sqrt(v_n * v_n + v_e * v_e); - double course = atan2(v_e, v_n); //Does this need to be in a specific range? All uses seem to accept anything. + double course = + atan2(v_e, v_n); //Does this need to be in a specific range? All uses seem to accept anything. input_.gps_Vg = ground_speed; - if(ground_speed > 0.3) //this is a magic number. What is it determined from? + if (ground_speed > 0.3) //this is a magic number. What is it determined from? input_.gps_course = course; } @@ -159,40 +160,32 @@ void estimator_base::imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) void estimator_base::baroAltCallback(const rosflight_msgs::msg::Barometer::SharedPtr msg) { - if (armed_first_time_ && !baro_init_) - { - if (baro_count_ < 100) - { + if (armed_first_time_ && !baro_init_) { + if (baro_count_ < 100) { init_static_ += msg->pressure; init_static_vector_.push_back(msg->pressure); input_.static_pres = 0; baro_count_ += 1; - } - else - { - init_static_ = std::accumulate(init_static_vector_.begin(), init_static_vector_.end(), - 0.0)/init_static_vector_.size(); + } else { + init_static_ = std::accumulate(init_static_vector_.begin(), init_static_vector_.end(), 0.0) + / init_static_vector_.size(); baro_init_ = true; //Check that it got a good calibration. std::sort(init_static_vector_.begin(), init_static_vector_.end()); - float q1 = (init_static_vector_[24] + init_static_vector_[25])/2.0; - float q3 = (init_static_vector_[74] + init_static_vector_[75])/2.0; + float q1 = (init_static_vector_[24] + init_static_vector_[25]) / 2.0; + float q3 = (init_static_vector_[74] + init_static_vector_[75]) / 2.0; float IQR = q3 - q1; - float upper_bound = q3 + 2.0*IQR; - float lower_bound = q1 - 2.0*IQR; - for (int i = 0; i < 100; i++) - { - if (init_static_vector_[i] > upper_bound) - { + float upper_bound = q3 + 2.0 * IQR; + float lower_bound = q1 - 2.0 * IQR; + for (int i = 0; i < 100; i++) { + if (init_static_vector_[i] > upper_bound) { baro_init_ = false; baro_count_ = 0; init_static_vector_.clear(); RCLCPP_WARN(this->get_logger(), "Bad baro calibration. Recalibrating"); break; - } - else if (init_static_vector_[i] < lower_bound) - { + } else if (init_static_vector_[i] < lower_bound) { baro_init_ = false; baro_count_ = 0; init_static_vector_.clear(); @@ -201,19 +194,14 @@ void estimator_base::baroAltCallback(const rosflight_msgs::msg::Barometer::Share } } } - } - else - { + } else { float static_pres_old = input_.static_pres; input_.static_pres = -msg->pressure + init_static_; - float gate_gain = 1.35*params_.rho*params_.gravity; - if (input_.static_pres < static_pres_old - gate_gain) - { + float gate_gain = 1.35 * params_.rho * params_.gravity; + if (input_.static_pres < static_pres_old - gate_gain) { input_.static_pres = static_pres_old - gate_gain; - } - else if (input_.static_pres > static_pres_old + gate_gain) - { + } else if (input_.static_pres > static_pres_old + gate_gain) { input_.static_pres = static_pres_old + gate_gain; } } @@ -224,26 +212,22 @@ void estimator_base::airspeedCallback(const rosflight_msgs::msg::Airspeed::Share float diff_pres_old = input_.diff_pres; input_.diff_pres = msg->differential_pressure; - float gate_gain = pow(5, 2)*params_.rho/2.0; - if (input_.diff_pres < diff_pres_old - gate_gain) - { + float gate_gain = pow(5, 2) * params_.rho / 2.0; + if (input_.diff_pres < diff_pres_old - gate_gain) { input_.diff_pres = diff_pres_old - gate_gain; - } - else if (input_.diff_pres > diff_pres_old + gate_gain) - { + } else if (input_.diff_pres > diff_pres_old + gate_gain) { input_.diff_pres = diff_pres_old + gate_gain; } } void estimator_base::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; } -} //end namespace -- +} // namespace rosplane -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 4e68401..0aa5ff4 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -1,41 +1,39 @@ -#include "estimator_base.hpp" #include "estimator_example.hpp" +#include "estimator_base.hpp" namespace rosplane { -float radians(float degrees) -{ - return M_PI*degrees/180.0; -} +float radians(float degrees) { return M_PI * degrees / 180.0; } -double wrap_within_180(double fixed_heading, double wrapped_heading) { - // wrapped_heading - number_of_times_to_wrap * 2pi - return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; +double wrap_within_180(double fixed_heading, double wrapped_heading) +{ + // wrapped_heading - number_of_times_to_wrap * 2pi + return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; } -estimator_example::estimator_example() : - estimator_base(), - xhat_a_(Eigen::Vector2f::Zero()), - P_a_(Eigen::Matrix2f::Zero()), - xhat_p_(Eigen::VectorXf::Zero(7)), - P_p_(Eigen::MatrixXf::Identity(7, 7)), - Q_a_(Eigen::Matrix2f::Identity()), - Q_g_(Eigen::Matrix3f::Identity()), - R_accel_(Eigen::Matrix3f::Identity()), - Q_p_(Eigen::MatrixXf::Identity(7, 7)), - R_p_(Eigen::MatrixXf::Zero(7, 7)), - f_p_(7), - A_p_(7, 7), - C_p_(7), - L_p_(7) +estimator_example::estimator_example() + : estimator_base() + , xhat_a_(Eigen::Vector2f::Zero()) + , P_a_(Eigen::Matrix2f::Zero()) + , xhat_p_(Eigen::VectorXf::Zero(7)) + , P_p_(Eigen::MatrixXf::Identity(7, 7)) + , Q_a_(Eigen::Matrix2f::Identity()) + , Q_g_(Eigen::Matrix3f::Identity()) + , R_accel_(Eigen::Matrix3f::Identity()) + , Q_p_(Eigen::MatrixXf::Identity(7, 7)) + , R_p_(Eigen::MatrixXf::Zero(7, 7)) + , f_p_(7) + , A_p_(7, 7) + , C_p_(7) + , L_p_(7) { P_a_ *= powf(radians(5.0f), 2); Q_a_(0, 0) = 0.01; Q_a_(1, 1) = 0.01; - Q_g_ *= pow(M_PI*.13/180.0,2); // TODO connect this to the actual params. + Q_g_ *= pow(M_PI * .13 / 180.0, 2); // TODO connect this to the actual params. P_p_ = Eigen::MatrixXf::Identity(7, 7); P_p_(0, 0) = .03; // TODO Add params for these? @@ -64,11 +62,11 @@ estimator_example::estimator_example() : alpha_ = 0.0f; } -void estimator_example::estimate(const params_s ¶ms, const input_s &input, output_s &output) +void estimator_example::estimate(const params_s & params, const input_s & input, output_s & output) { if (alpha_ == 0.0f) //initailze stuff that comes from params { - R_accel_ = R_accel_ * pow(.0025*9.81, 2); // TODO add ot params + R_accel_ = R_accel_ * pow(.0025 * 9.81, 2); // TODO add ot params R_p_(0, 0) = powf(params.sigma_n_gps, 2); R_p_(1, 1) = powf(params.sigma_e_gps, 2); @@ -78,45 +76,44 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o R_p_(5, 5) = 0.01; R_p_(6, 6) = 0.01; - float lpf_a = 50.0; float lpf_a1 = 8.0; - alpha_ = exp(-lpf_a*params.Ts); - alpha1_ = exp(-lpf_a1*params.Ts); + alpha_ = exp(-lpf_a * params.Ts); + alpha1_ = exp(-lpf_a1 * params.Ts); } // low pass filter gyros to estimate angular rates - lpf_gyro_x_ = alpha_*lpf_gyro_x_ + (1 - alpha_)*input.gyro_x; - lpf_gyro_y_ = alpha_*lpf_gyro_y_ + (1 - alpha_)*input.gyro_y; - lpf_gyro_z_ = alpha_*lpf_gyro_z_ + (1 - alpha_)*input.gyro_z; + lpf_gyro_x_ = alpha_ * lpf_gyro_x_ + (1 - alpha_) * input.gyro_x; + lpf_gyro_y_ = alpha_ * lpf_gyro_y_ + (1 - alpha_) * input.gyro_y; + lpf_gyro_z_ = alpha_ * lpf_gyro_z_ + (1 - alpha_) * input.gyro_z; float phat = lpf_gyro_x_; float qhat = lpf_gyro_y_; float rhat = lpf_gyro_z_; // low pass filter static pressure sensor and invert to esimate altitude - lpf_static_ = alpha1_*lpf_static_ + (1 - alpha1_)*input.static_pres; - float hhat = lpf_static_/params.rho/params.gravity; + lpf_static_ = alpha1_ * lpf_static_ + (1 - alpha1_) * input.static_pres; + float hhat = lpf_static_ / params.rho / params.gravity; - if(input.static_pres == 0.0 || baro_init_ == false){ // Catch the edge case for if pressure measured is zero. + if (input.static_pres == 0.0 + || baro_init_ == false) { // Catch the edge case for if pressure measured is zero. hhat = 0.0; } // low pass filter diff pressure sensor and invert to estimate Va - lpf_diff_ = alpha1_*lpf_diff_ + (1 - alpha1_)*input.diff_pres; + lpf_diff_ = alpha1_ * lpf_diff_ + (1 - alpha1_) * input.diff_pres; // 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/params.rho*lpf_diff_); + float Vahat = sqrt(2 / params.rho * lpf_diff_); // low pass filter accelerometers - 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; + 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; // implement continuous-discrete EKF to estimate roll and pitch angles // prediction step @@ -125,18 +122,17 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o float tt; // tan(thata) float ct; // cos(thata)F float st; // sin(theta) - for (int i = 0; i < N_; i++) - { + for (int i = 0; i < N_; i++) { cp = cosf(xhat_a_(0)); // cos(phi) sp = sinf(xhat_a_(0)); // sin(phi) tt = tanf(xhat_a_(1)); // tan(theta) ct = cosf(xhat_a_(1)); // cos(theta) - f_a_(0) = phat + (qhat*sp + rhat*cp)*tt; - f_a_(1) = qhat*cp - rhat*sp; + f_a_(0) = phat + (qhat * sp + rhat * cp) * tt; + f_a_(1) = qhat * cp - rhat * sp; - xhat_a_ += f_a_*(params.Ts/N_); + xhat_a_ += f_a_ * (params.Ts / N_); cp = cosf(xhat_a_(0)); // cos(phi) sp = sinf(xhat_a_(0)); // sin(phi) @@ -144,19 +140,18 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o ct = cosf(xhat_a_(1)); // cos(theta) A_a_ = Eigen::Matrix2f::Zero(); - A_a_(0, 0) = (qhat*cp - rhat*sp)*tt; - A_a_(0, 1) = (qhat*sp + rhat*cp)/ct/ct; - A_a_(1, 0) = -qhat*sp - rhat*cp; + A_a_(0, 0) = (qhat * cp - rhat * sp) * tt; + A_a_(0, 1) = (qhat * sp + rhat * cp) / ct / ct; + A_a_(1, 0) = -qhat * sp - rhat * cp; - Eigen::MatrixXf A_d = Eigen::MatrixXf::Identity(2,2) + params.Ts/N_ * A_a_ + pow(params.Ts/N_, 2) / 2.0 * A_a_ * A_a_; + Eigen::MatrixXf A_d = Eigen::MatrixXf::Identity(2, 2) + params.Ts / N_ * A_a_ + + pow(params.Ts / N_, 2) / 2.0 * A_a_ * A_a_; Eigen::Matrix G; - G << 1, sp*tt, cp*tt, 0.0, cp, -sp; - - P_a_ = (A_d*P_a_*A_d.transpose() + (Q_a_ + G * Q_g_ * G.transpose())*pow(params.Ts/N_, 2)); - - + G << 1, sp * tt, cp * tt, 0.0, cp, -sp; + P_a_ = + (A_d * P_a_ * A_d.transpose() + (Q_a_ + G * Q_g_ * G.transpose()) * pow(params.Ts / N_, 2)); } // measurement updates cp = cosf(xhat_a_(0)); @@ -167,13 +162,13 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o I = Eigen::Matrix2f::Identity(); h_a_ = Eigen::Vector3f::Zero(3); - h_a_(0) = qhat * Vahat * st + params.gravity*st; + h_a_(0) = qhat * Vahat * st + params.gravity * st; h_a_(1) = rhat * Vahat * ct - phat * Vahat * st - params.gravity * ct * sp; h_a_(2) = -qhat * Vahat * ct - params.gravity * ct * cp; - C_a_ << 0.0, qhat*Vahat*ct + params.gravity*ct, - -params.gravity*cp*ct, -rhat*Vahat*st - phat*Vahat*ct + params.gravity*sp*st, - params.gravity*sp*ct, (qhat*Vahat + params.gravity*cp) * st; + C_a_ << 0.0, qhat * Vahat * ct + params.gravity * ct, -params.gravity * cp * ct, + -rhat * Vahat * st - phat * Vahat * ct + params.gravity * sp * st, params.gravity * sp * ct, + (qhat * Vahat + params.gravity * cp) * st; // This calculates the Kalman Gain for all of the attitude states at once rather than one at a time. @@ -183,9 +178,9 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o Eigen::MatrixXf S_inv = (R_accel_ + C_a_ * P_a_ * C_a_.transpose()).inverse(); Eigen::MatrixXf L_a_ = P_a_ * C_a_.transpose() * S_inv; - Eigen::MatrixXf temp = Eigen::MatrixXf::Identity(2,2) - L_a_ * C_a_; + Eigen::MatrixXf temp = Eigen::MatrixXf::Identity(2, 2) - L_a_ * C_a_; - P_a_ = temp * P_a_ * temp.transpose() + L_a_ * R_accel_ *L_a_.transpose(); + P_a_ = temp * P_a_ * temp.transpose() + L_a_ * R_accel_ * L_a_.transpose(); xhat_a_ = xhat_a_ + L_a_ * (y - h_a_); check_xhat_a(); @@ -196,13 +191,11 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o // implement continous-discrete EKF to estimate pn, pe, chi, Vg // prediction step float psidot, tmp, Vgdot; - if (fabsf(xhat_p_(2)) < 0.01f) - { + if (fabsf(xhat_p_(2)) < 0.01f) { xhat_p_(2) = 0.01; // prevent divide by zero } - for (int i = 0; i < N_; i++) - { + for (int i = 0; i < N_; i++) { float Vg = xhat_p_(2); float chi = xhat_p_(3); @@ -210,56 +203,53 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o float we = xhat_p_(5); float psi = xhat_p_(6); - psidot = (qhat*sinf(phihat_) + rhat*cosf(phihat_))/cosf(thetahat_); + psidot = (qhat * sinf(phihat_) + rhat * cosf(phihat_)) / cosf(thetahat_); - tmp = -psidot*Vahat*(xhat_p_(4)*cosf(xhat_p_(6)) + xhat_p_(5)*sinf(xhat_p_(6)))/xhat_p_(2); - Vgdot = Vahat/Vg * psidot*(we*cosf(psi) - wn*sinf(psi)); + tmp = -psidot * Vahat * (xhat_p_(4) * cosf(xhat_p_(6)) + xhat_p_(5) * sinf(xhat_p_(6))) + / xhat_p_(2); + Vgdot = Vahat / Vg * psidot * (we * cosf(psi) - wn * sinf(psi)); f_p_ = Eigen::VectorXf::Zero(7); - f_p_(0) = xhat_p_(2)*cosf(xhat_p_(3)); - f_p_(1) = xhat_p_(2)*sinf(xhat_p_(3)); + f_p_(0) = xhat_p_(2) * cosf(xhat_p_(3)); + f_p_(1) = xhat_p_(2) * sinf(xhat_p_(3)); f_p_(2) = Vgdot; - f_p_(3) = params.gravity/xhat_p_(2)*tanf(phihat_)*cosf(chi-psi); + f_p_(3) = params.gravity / xhat_p_(2) * tanf(phihat_) * cosf(chi - psi); f_p_(6) = psidot; - xhat_p_ += f_p_*(params.Ts/N_); + xhat_p_ += f_p_ * (params.Ts / N_); A_p_ = Eigen::MatrixXf::Zero(7, 7); A_p_(0, 2) = cos(xhat_p_(3)); - A_p_(0, 3) = -xhat_p_(2)*sinf(xhat_p_(3)); + A_p_(0, 3) = -xhat_p_(2) * sinf(xhat_p_(3)); A_p_(1, 2) = sin(xhat_p_(3)); - A_p_(1, 3) = xhat_p_(2)*cosf(xhat_p_(3)); - A_p_(2, 2) = -Vgdot/xhat_p_(2); - A_p_(2, 4) = -psidot*Vahat*sinf(xhat_p_(6))/xhat_p_(2); - A_p_(2, 5) = psidot*Vahat*cosf(xhat_p_(6))/xhat_p_(2); + A_p_(1, 3) = xhat_p_(2) * cosf(xhat_p_(3)); + A_p_(2, 2) = -Vgdot / xhat_p_(2); + A_p_(2, 4) = -psidot * Vahat * sinf(xhat_p_(6)) / xhat_p_(2); + A_p_(2, 5) = psidot * Vahat * cosf(xhat_p_(6)) / xhat_p_(2); A_p_(2, 6) = tmp; - A_p_(3, 2) = -params.gravity/powf(xhat_p_(2), 2)*tanf(phihat_); - + A_p_(3, 2) = -params.gravity / powf(xhat_p_(2), 2) * tanf(phihat_); - Eigen::MatrixXf A_d_ = Eigen::MatrixXf::Identity(7,7) + params.Ts/N_*A_p_ + A_p_ * A_p_ * pow(params.Ts/N_, 2)/2.0; + Eigen::MatrixXf A_d_ = Eigen::MatrixXf::Identity(7, 7) + params.Ts / N_ * A_p_ + + A_p_ * A_p_ * pow(params.Ts / N_, 2) / 2.0; - P_p_ = (A_d_*P_p_*A_d_.transpose() + Q_p_*pow(params.Ts/N_, 2)); + P_p_ = (A_d_ * P_p_ * A_d_.transpose() + Q_p_ * pow(params.Ts / N_, 2)); } - - xhat_p_(3) = wrap_within_180(0.0, xhat_p_(3)); - if(xhat_p_(3) > radians(180.0f) || xhat_p_(3) < radians(-180.0f)) - { - RCLCPP_WARN(this->get_logger(), "Course estimate not wrapped from -pi to pi"); - xhat_p_(3) = 0; - } - xhat_p_(6) = wrap_within_180(0.0, xhat_p_(6)); - if(xhat_p_(6) > radians(180.0f) || xhat_p_(6) < radians(-180.0f)) - { - RCLCPP_WARN(this->get_logger(), "Psi estimate not wrapped from -pi to pi"); - xhat_p_(6) = 0; - } + xhat_p_(3) = wrap_within_180(0.0, xhat_p_(3)); + if (xhat_p_(3) > radians(180.0f) || xhat_p_(3) < radians(-180.0f)) { + RCLCPP_WARN(this->get_logger(), "Course estimate not wrapped from -pi to pi"); + xhat_p_(3) = 0; + } + xhat_p_(6) = wrap_within_180(0.0, xhat_p_(6)); + if (xhat_p_(6) > radians(180.0f) || xhat_p_(6) < radians(-180.0f)) { + RCLCPP_WARN(this->get_logger(), "Psi estimate not wrapped from -pi to pi"); + xhat_p_(6) = 0; + } // measurement updates // These calculate the Kalman gain and applies them to each state individually. - if (input.gps_new) - { + if (input.gps_new) { Eigen::MatrixXf I_p(7, 7); I_p = Eigen::MatrixXf::Identity(7, 7); @@ -268,25 +258,25 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o h_p_ = xhat_p_(0); C_p_ = Eigen::VectorXf::Zero(7); C_p_(0) = 1; - L_p_ = (P_p_*C_p_)/(R_p_(0, 0) + (C_p_.transpose()*P_p_*C_p_)); - P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; - xhat_p_ = xhat_p_ + L_p_*(input.gps_n - h_p_); + L_p_ = (P_p_ * C_p_) / (R_p_(0, 0) + (C_p_.transpose() * P_p_ * C_p_)); + P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; + xhat_p_ = xhat_p_ + L_p_ * (input.gps_n - h_p_); // gps East position h_p_ = xhat_p_(1); C_p_ = Eigen::VectorXf::Zero(7); C_p_(1) = 1; - L_p_ = (P_p_*C_p_)/(R_p_(1, 1) + (C_p_.transpose()*P_p_*C_p_)); - P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; - xhat_p_ = xhat_p_ + L_p_*(input.gps_e - h_p_); + L_p_ = (P_p_ * C_p_) / (R_p_(1, 1) + (C_p_.transpose() * P_p_ * C_p_)); + P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; + xhat_p_ = xhat_p_ + L_p_ * (input.gps_e - h_p_); // gps ground speed h_p_ = xhat_p_(2); C_p_ = Eigen::VectorXf::Zero(7); C_p_(2) = 1; - L_p_ = (P_p_*C_p_)/(R_p_(2, 2) + (C_p_.transpose()*P_p_*C_p_)); - P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; - xhat_p_ = xhat_p_ + L_p_*(input.gps_Vg - h_p_); + L_p_ = (P_p_ * C_p_) / (R_p_(2, 2) + (C_p_.transpose() * P_p_ * C_p_)); + P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; + xhat_p_ = xhat_p_ + L_p_ * (input.gps_Vg - h_p_); // gps course //wrap course measurement @@ -297,40 +287,39 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o C_p_ = Eigen::VectorXf::Zero(7); C_p_(3) = 1; - L_p_ = (P_p_*C_p_)/(R_p_(3, 3) + (C_p_.transpose()*P_p_*C_p_)); - P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; - xhat_p_ = xhat_p_ + L_p_*(gps_course - h_p_); + L_p_ = (P_p_ * C_p_) / (R_p_(3, 3) + (C_p_.transpose() * P_p_ * C_p_)); + P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; + xhat_p_ = xhat_p_ + L_p_ * (gps_course - h_p_); // pseudo measurement #1 y_1 = Va*cos(psi)+wn-Vg*cos(chi) - h_p_ = Vahat*cosf(xhat_p_(6)) + xhat_p_(4) - xhat_p_(2)*cosf(xhat_p_(3)); // pseudo measurement + h_p_ = + Vahat * cosf(xhat_p_(6)) + xhat_p_(4) - xhat_p_(2) * cosf(xhat_p_(3)); // pseudo measurement C_p_ = Eigen::VectorXf::Zero(7); C_p_(2) = -cos(xhat_p_(3)); - C_p_(3) = xhat_p_(2)*sinf(xhat_p_(3)); + C_p_(3) = xhat_p_(2) * sinf(xhat_p_(3)); C_p_(4) = 1; - C_p_(6) = -Vahat*sinf(xhat_p_(6)); - L_p_ = (P_p_*C_p_)/(R_p_(4,4) + (C_p_.transpose()*P_p_*C_p_)); - P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; - xhat_p_ = xhat_p_ + L_p_*(0 - h_p_); - + C_p_(6) = -Vahat * sinf(xhat_p_(6)); + L_p_ = (P_p_ * C_p_) / (R_p_(4, 4) + (C_p_.transpose() * P_p_ * C_p_)); + P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; + xhat_p_ = xhat_p_ + L_p_ * (0 - h_p_); // pseudo measurement #2 y_2 = Va*sin(psi) + we - Vg*sin(chi) - h_p_ = Vahat*sinf(xhat_p_(6))+xhat_p_(5)-xhat_p_(2)*sinf(xhat_p_(3)); // pseudo measurement + h_p_ = + Vahat * sinf(xhat_p_(6)) + xhat_p_(5) - xhat_p_(2) * sinf(xhat_p_(3)); // pseudo measurement C_p_ = Eigen::VectorXf::Zero(7); C_p_(2) = -sin(xhat_p_(3)); - C_p_(3) = -xhat_p_(2)*cosf(xhat_p_(3)); + C_p_(3) = -xhat_p_(2) * cosf(xhat_p_(3)); C_p_(5) = 1; - C_p_(6) = Vahat*cosf(xhat_p_(6)); - L_p_ = (P_p_*C_p_)/(R_p_(5,5) + (C_p_.transpose()*P_p_*C_p_)); - P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; - xhat_p_ = xhat_p_ + L_p_*(0 - h_p_); + C_p_(6) = Vahat * cosf(xhat_p_(6)); + L_p_ = (P_p_ * C_p_) / (R_p_(5, 5) + (C_p_.transpose() * P_p_ * C_p_)); + P_p_ = (I_p - L_p_ * C_p_.transpose()) * P_p_; + xhat_p_ = xhat_p_ + L_p_ * (0 - h_p_); - if (xhat_p_(0) > 10000 || xhat_p_(0) < -10000) - { + if (xhat_p_(0) > 10000 || xhat_p_(0) < -10000) { RCLCPP_WARN(this->get_logger(), "gps n limit reached"); xhat_p_(0) = input.gps_n; } - if (xhat_p_(1) > 10000 || xhat_p_(1) < -10000) - { + if (xhat_p_(1) > 10000 || xhat_p_(1) < -10000) { RCLCPP_WARN(this->get_logger(), "gps e limit reached"); xhat_p_(1) = input.gps_e; } @@ -338,34 +327,30 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o bool problem = false; int prob_index; - for (int i = 0; i < 7; i++) - { - if (!std::isfinite(xhat_p_(i))) - { - if (!problem) - { + for (int i = 0; i < 7; i++) { + if (!std::isfinite(xhat_p_(i))) { + if (!problem) { problem = true; prob_index = i; } - switch (i) - { - case 0: - xhat_p_(i) = input.gps_n; - break; - case 1: - xhat_p_(i) = input.gps_e; - break; - case 2: - xhat_p_(i) = input.gps_Vg; - break; - case 3: - xhat_p_(i) = input.gps_course; - break; - case 6: - xhat_p_(i) = input.gps_course; - break; - default: - xhat_p_(i) = 0; + switch (i) { + case 0: + xhat_p_(i) = input.gps_n; + break; + case 1: + xhat_p_(i) = input.gps_e; + break; + case 2: + xhat_p_(i) = input.gps_Vg; + break; + case 3: + xhat_p_(i) = input.gps_course; + break; + case 6: + xhat_p_(i) = input.gps_course; + break; + default: + xhat_p_(i) = 0; } P_p_ = Eigen::MatrixXf::Identity(7, 7); P_p_(0, 0) = .03; @@ -377,14 +362,13 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o P_p_(6, 6) = radians(5.0f); } } - if (problem) - { - RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", prob_index); + if (problem) { + RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", + prob_index); } - if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f)) - { + if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f)) { //xhat_p(3) = fmodf(xhat_p(3),2.0*M_PI); - xhat_p_(6) = fmodf(xhat_p_(6), 2.0*M_PI); + xhat_p_(6) = fmodf(xhat_p_(6), 2.0 * M_PI); } float pnhat = xhat_p_(0); @@ -415,46 +399,34 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o void estimator_example::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))) - { + 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(85.0)) - { + } else if (xhat_a_(0) > radians(85.0)) { xhat_a_(0) = radians(82.0); RCLCPP_WARN(this->get_logger(), "max roll angle"); - } - else if (xhat_a_(0) < radians(-85.0)) - { + } else if (xhat_a_(0) < radians(-85.0)) { xhat_a_(0) = radians(-82.0); RCLCPP_WARN(this->get_logger(), "min roll angle"); } } - if (xhat_a_(1) > radians(80.0) || xhat_a_(1) < radians(-80.0) || !std::isfinite(xhat_a_(1))) - { - if (!std::isfinite(xhat_a_(1))) - { + if (xhat_a_(1) > radians(80.0) || xhat_a_(1) < radians(-80.0) || !std::isfinite(xhat_a_(1))) { + if (!std::isfinite(xhat_a_(1))) { xhat_a_(1) = 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 pitch"); - } - else if (xhat_a_(1) > radians(80.0)) - { + } else if (xhat_a_(1) > radians(80.0)) { xhat_a_(1) = radians(77.0); RCLCPP_WARN(this->get_logger(), "max pitch angle"); - } - else if (xhat_a_(1) < radians(-80.0)) - { + } else if (xhat_a_(1) < radians(-80.0)) { xhat_a_(1) = radians(-77.0); RCLCPP_WARN(this->get_logger(), "min pitch angle"); } } } -} //end namespace +} // namespace rosplane diff --git a/rosplane/src/path_follower_base.cpp b/rosplane/src/path_follower_base.cpp index 94cb885..ae0d6e1 100644 --- a/rosplane/src/path_follower_base.cpp +++ b/rosplane/src/path_follower_base.cpp @@ -5,20 +5,24 @@ namespace rosplane { -path_follower_base::path_follower_base(): Node("path_follower_base") +path_follower_base::path_follower_base() + : Node("path_follower_base") { vehicle_state_sub_ = this->create_subscription( - "estimated_state", 10, std::bind(&path_follower_base::vehicle_state_callback, this, _1)); + "estimated_state", 10, std::bind(&path_follower_base::vehicle_state_callback, this, _1)); current_path_sub_ = this->create_subscription( - "current_path", 100, std::bind(&path_follower_base::current_path_callback, this, _1)); // the 1 may need to be 100 - - - update_timer_ = this->create_wall_timer(100ms, std::bind(&path_follower_base::update, this)); // TODO change this duration to change based on update rate. - controller_commands_pub_ = this->create_publisher("controller_commands",1); + "current_path", 100, + std::bind(&path_follower_base::current_path_callback, this, _1)); // the 1 may need to be 100 + update_timer_ = this->create_wall_timer( + 100ms, + std::bind(&path_follower_base::update, + this)); // TODO change this duration to change based on update rate. + controller_commands_pub_ = + this->create_publisher("controller_commands", 1); parameter_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&path_follower_base::parametersCallback, this, std::placeholders::_1)); + std::bind(&path_follower_base::parametersCallback, this, std::placeholders::_1)); this->declare_parameter("CHI_INFTY", params_.chi_infty); this->declare_parameter("K_PATH", params_.k_path); @@ -37,8 +41,7 @@ void path_follower_base::update() struct output_s output; - if (state_init_ == true && current_path_init_ == true) - { + if (state_init_ == true && current_path_init_ == true) { follow(params_, input_, output); rosplane_msgs::msg::ControllerCommands msg; @@ -51,7 +54,6 @@ void path_follower_base::update() msg.h_c = output.h_c; msg.phi_ff = output.phi_ff; - RCLCPP_DEBUG_STREAM(this->get_logger(), "Publishing Contoller Commands!"); RCLCPP_DEBUG_STREAM(this->get_logger(), "chi_c: " << msg.chi_c); @@ -61,37 +63,32 @@ void path_follower_base::update() RCLCPP_DEBUG_STREAM(this->get_logger(), "k_orbit: " << params_.k_orbit); - - controller_commands_pub_->publish(msg); } } void path_follower_base::vehicle_state_callback(const rosplane_msgs::msg::State::SharedPtr msg) { - input_.pn = msg->position[0]; /** position north */ - input_.pe = msg->position[1]; /** position east */ - input_.h = -msg->position[2]; /** altitude */ + input_.pn = msg->position[0]; /** position north */ + input_.pe = msg->position[1]; /** position east */ + input_.h = -msg->position[2]; /** altitude */ input_.chi = msg->chi; input_.psi = msg->psi; input_.Va = msg->va; RCLCPP_DEBUG_STREAM(this->get_logger(), "FROM STATE -- input.chi: " << input_.chi); - state_init_ = true; } void path_follower_base::current_path_callback(const rosplane_msgs::msg::CurrentPath::SharedPtr msg) { - if (msg->path_type == msg->LINE_PATH) - input_.p_type = path_type::Line; + if (msg->path_type == msg->LINE_PATH) input_.p_type = path_type::Line; else if (msg->path_type == msg->ORBIT_PATH) input_.p_type = path_type::Orbit; input_.Va_d = msg->va_d; - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 3; i++) { input_.r_path[i] = msg->r[i]; input_.q_path[i] = msg->q[i]; input_.c_orbit[i] = msg->c[i]; @@ -101,39 +98,38 @@ void path_follower_base::current_path_callback(const rosplane_msgs::msg::Current current_path_init_ = true; } -rcl_interfaces::msg::SetParametersResult path_follower_base::parametersCallback(const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = false; - result.reason = ""; - - for (const auto ¶m : parameters) { - - if (param.get_name() == "CHI_INFTY"){ - params_.chi_infty = param.as_double(); - result.successful = true; - result.reason = "success"; - } - else if (param.get_name() == "K_PATH"){ - RCLCPP_INFO_STREAM(this->get_logger(), "K_PATH Orginal: " << params_.k_path); - params_.k_path = param.as_double(); - result.successful = true; - result.reason = "success"; - RCLCPP_INFO_STREAM(this->get_logger(), "K_PATH Changed: " << params_.k_path); - } - else if (param.get_name() == "K_ORBIT"){ - params_.k_orbit = param.as_double(); - result.successful = true; - result.reason = "success"; - } - +rcl_interfaces::msg::SetParametersResult +path_follower_base::parametersCallback(const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = ""; + + for (const auto & param : parameters) { + + if (param.get_name() == "CHI_INFTY") { + params_.chi_infty = param.as_double(); + result.successful = true; + result.reason = "success"; + } else if (param.get_name() == "K_PATH") { + RCLCPP_INFO_STREAM(this->get_logger(), "K_PATH Orginal: " << params_.k_path); + params_.k_path = param.as_double(); + result.successful = true; + result.reason = "success"; + RCLCPP_INFO_STREAM(this->get_logger(), "K_PATH Changed: " << params_.k_path); + } else if (param.get_name() == "K_ORBIT") { + params_.k_orbit = param.as_double(); + result.successful = true; + result.reason = "success"; } + } - return result; + return result; } -} //end namespace +} // namespace rosplane -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/rosplane/src/path_follower_example.cpp b/rosplane/src/path_follower_example.cpp index b6f566f..b46960f 100644 --- a/rosplane/src/path_follower_example.cpp +++ b/rosplane/src/path_follower_example.cpp @@ -1,69 +1,70 @@ #include "path_follower_example.hpp" #include - namespace rosplane { -double wrap_within_180(double fixed_heading, double wrapped_heading) { - // wrapped_heading - number_of_times_to_wrap * 2pi - return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; -} - -path_follower_example::path_follower_example() +double wrap_within_180(double fixed_heading, double wrapped_heading) { + // wrapped_heading - number_of_times_to_wrap * 2pi + return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; } -void path_follower_example::follow(const params_s ¶ms, const input_s &input, output_s &output) +path_follower_example::path_follower_example() {} + +void path_follower_example::follow(const params_s & params, const input_s & input, + output_s & output) { if (input.p_type == path_type::Line) // follow straight line path specified by r and q { // compute wrapped version of the path angle float chi_q = atan2f(input.q_path[1], input.q_path[0]); - + chi_q = wrap_within_180(input.chi, chi_q); RCLCPP_DEBUG_STREAM(this->get_logger(), "input.chi: " << input.chi); RCLCPP_DEBUG_STREAM(this->get_logger(), "chi_q: " << chi_q); - - float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]); + float path_error = + -sinf(chi_q) * (input.pn - input.r_path[0]) + cosf(chi_q) * (input.pe - input.r_path[1]); // RCLCPP_INFO_STREAM(this->get_logger(), "k_path: " << params.k_path); // heading command - output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params_.k_path*path_error); + output.chi_c = chi_q - params.chi_infty * 2 / M_PI * atanf(params_.k_path * path_error); // desired altitude - float h_d = -input.r_path[2] - sqrtf(powf((input.r_path[0] - input.pn), 2) + powf((input.r_path[1] - input.pe), - 2))*(input.q_path[2])/sqrtf(powf(input.q_path[0], 2) + powf(input.q_path[1], 2)); + float h_d = -input.r_path[2] + - sqrtf(powf((input.r_path[0] - input.pn), 2) + powf((input.r_path[1] - input.pe), 2)) + * (input.q_path[2]) / sqrtf(powf(input.q_path[0], 2) + powf(input.q_path[1], 2)); // commanded altitude is desired altitude output.h_c = h_d; output.phi_ff = 0.0; - } - else // follow an orbit path specified by c_orbit, rho_orbit, and lam_orbit + } else // follow an orbit path specified by c_orbit, rho_orbit, and lam_orbit { - float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + powf((input.pe - input.c_orbit[1]), - 2)); // distance from orbit center + float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + + powf((input.pe - input.c_orbit[1]), + 2)); // distance from orbit center // compute wrapped version of angular position on orbit float varphi = atan2f(input.pe - input.c_orbit[1], input.pn - input.c_orbit[0]); - + varphi = wrap_within_180(input.chi, varphi); //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(params.k_orbit*norm_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(params.k_orbit * norm_orbit_error)); // commanded altitude is the height of the orbit float h_d = -input.c_orbit[2]; output.h_c = h_d; - output.phi_ff = input.lam_orbit * std::atan(pow(input.Va, 2)/(9.81*input.rho_orbit*std::cos(input.chi - input.psi))); + output.phi_ff = input.lam_orbit + * std::atan(pow(input.Va, 2) / (9.81 * input.rho_orbit * std::cos(input.chi - input.psi))); output.chi_c = wrap_within_180(0.0, output.chi_c); - } output.Va_c = input.Va_d; } -} //end namespace +} // namespace rosplane diff --git a/rosplane/src/path_manager_base.cpp b/rosplane/src/path_manager_base.cpp old mode 100755 new mode 100644 index cc9091d..3782a5b --- a/rosplane/src/path_manager_base.cpp +++ b/rosplane/src/path_manager_base.cpp @@ -1,25 +1,28 @@ #include "path_manager_base.hpp" +#include "iostream" #include "path_manager_example.hpp" #include "rclcpp/rclcpp.hpp" -#include "iostream" #include - namespace rosplane { -path_manager_base::path_manager_base() : Node("rosplane_path_manager") +path_manager_base::path_manager_base() + : Node("rosplane_path_manager") { - vehicle_state_sub_ = this->create_subscription("estimated_state", 10, std::bind(&path_manager_base::vehicle_state_callback,this,_1)); - new_waypoint_sub_ = this->create_subscription("waypoint_path", 10, std::bind(&path_manager_base::new_waypoint_callback,this,_1)); - current_path_pub_ = this->create_publisher("current_path", 10); - update_timer_ = this->create_wall_timer(10ms, std::bind(&path_manager_base::current_path_publish, this)); + vehicle_state_sub_ = this->create_subscription( + "estimated_state", 10, std::bind(&path_manager_base::vehicle_state_callback, this, _1)); + new_waypoint_sub_ = this->create_subscription( + "waypoint_path", 10, std::bind(&path_manager_base::new_waypoint_callback, this, _1)); + current_path_pub_ = this->create_publisher("current_path", 10); + update_timer_ = + this->create_wall_timer(10ms, std::bind(&path_manager_base::current_path_publish, this)); // interesting read on wall timer // https://answers.ros.org/question/375561/create-wall-timer-using-callback-with-parameters-ros2-c/ // // Set the parameter callback, for when parameters are changed. parameter_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&path_manager_base::parametersCallback, this, std::placeholders::_1)); + std::bind(&path_manager_base::parametersCallback, this, std::placeholders::_1)); this->declare_parameter("R_min", 25.0); this->declare_parameter("orbit_last", false); @@ -32,30 +35,33 @@ path_manager_base::path_manager_base() : Node("rosplane_path_manager") state_init_ = false; } -rcl_interfaces::msg::SetParametersResult path_manager_base::parametersCallback(const std::vector ¶meters) { +rcl_interfaces::msg::SetParametersResult +path_manager_base::parametersCallback(const std::vector & parameters) +{ rcl_interfaces::msg::SetParametersResult result; result.successful = true; result.reason = "success"; // Check each parameter in the incoming vector of parameters to change, and change the appropriate parameter. - for (const auto ¶m : parameters) { + for (const auto & param : parameters) { if (param.get_name() == "R_min") params_.R_min = param.as_double(); - else if (param.get_name() == "orbit_last") params_.orbit_last = param.as_bool(); - else{ + else if (param.get_name() == "orbit_last") + params_.orbit_last = param.as_bool(); + else { // If the parameter given doesn't match any of the parameters return false. 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."; break; } - } return result; } -void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State &msg) +void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State & msg) { vehicle_state_ = msg; @@ -63,17 +69,15 @@ void path_manager_base::vehicle_state_callback(const rosplane_msgs::msg::State & state_init_ = true; } -void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint &msg) +void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint & msg) { - if (msg.clear_wp_list == true) - { + if (msg.clear_wp_list == true) { waypoints_.clear(); num_waypoints_ = 0; idx_a_ = 0; return; } - if (msg.set_current || num_waypoints_ == 0) - { + if (msg.set_current || num_waypoints_ == 0) { waypoint_s currentwp; currentwp.w[0] = vehicle_state_.position[0]; currentwp.w[1] = vehicle_state_.position[1]; @@ -89,32 +93,28 @@ void path_manager_base::new_waypoint_callback(const rosplane_msgs::msg::Waypoint idx_a_ = 0; } waypoint_s nextwp; - nextwp.w[0] = msg.w[0]; - nextwp.w[1] = msg.w[1]; - nextwp.w[2] = msg.w[2]; - nextwp.chi_d = msg.chi_d; - nextwp.chi_valid = msg.chi_valid; - nextwp.va_d = msg.va_d; + nextwp.w[0] = msg.w[0]; + nextwp.w[1] = msg.w[1]; + nextwp.w[2] = msg.w[2]; + nextwp.chi_d = msg.chi_d; + nextwp.chi_valid = msg.chi_valid; + nextwp.va_d = msg.va_d; waypoints_.push_back(nextwp); num_waypoints_++; } - void path_manager_base::current_path_publish() //const rclcpp::TimerEvent & { struct input_s input; - input.pn = vehicle_state_.position[0]; // position north - input.pe = vehicle_state_.position[1]; // position east - input.h = -vehicle_state_.position[2]; // altitude + input.pn = vehicle_state_.position[0]; // position north + input.pe = vehicle_state_.position[1]; // position east + input.h = -vehicle_state_.position[2]; // altitude input.chi = vehicle_state_.chi; struct output_s output; - if (state_init_ == true) - { - manage(params_, input, output); - } + if (state_init_ == true) { manage(params_, input, output); } rosplane_msgs::msg::CurrentPath current_path; @@ -122,13 +122,11 @@ void path_manager_base::current_path_publish() //const rclcpp::TimerEvent & current_path.header.stamp = now; - if (output.flag) - current_path.path_type = current_path.LINE_PATH; + if (output.flag) current_path.path_type = current_path.LINE_PATH; else current_path.path_type = current_path.ORBIT_PATH; current_path.va_d = output.va_d; - for (int i = 0; i < 3; i++) - { + for (int i = 0; i < 3; i++) { current_path.r[i] = output.r[i]; current_path.q[i] = output.q[i]; current_path.c[i] = output.c[i]; @@ -140,21 +138,24 @@ void path_manager_base::current_path_publish() //const rclcpp::TimerEvent & RCLCPP_DEBUG_STREAM(this->get_logger(), "Path Type: " << current_path.path_type); RCLCPP_DEBUG_STREAM(this->get_logger(), "va_d: " << current_path.va_d); - RCLCPP_DEBUG_STREAM(this->get_logger(), "r: " << current_path.r[0] << ", " << current_path.r[1] << ", " << current_path.r[2]); - RCLCPP_DEBUG_STREAM(this->get_logger(), "q: " << current_path.q[0] << ", " << current_path.q[1] << ", " << current_path.q[2]); - RCLCPP_DEBUG_STREAM(this->get_logger(), "c: " << current_path.c[0] << ", " << current_path.c[1] << ", " << current_path.c[2]); + RCLCPP_DEBUG_STREAM(this->get_logger(), + "r: " << current_path.r[0] << ", " << current_path.r[1] << ", " + << current_path.r[2]); + RCLCPP_DEBUG_STREAM(this->get_logger(), + "q: " << current_path.q[0] << ", " << current_path.q[1] << ", " + << current_path.q[2]); + RCLCPP_DEBUG_STREAM(this->get_logger(), + "c: " << current_path.c[0] << ", " << current_path.c[1] << ", " + << current_path.c[2]); RCLCPP_DEBUG_STREAM(this->get_logger(), "rho: " << current_path.rho); RCLCPP_DEBUG_STREAM(this->get_logger(), "lamda: " << current_path.lamda); - current_path_pub_->publish(current_path); -} - - -} //end namespace +} +} // namespace rosplane -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/rosplane/src/path_manager_example.cpp b/rosplane/src/path_manager_example.cpp old mode 100755 new mode 100644 index f1d30d7..a49a97f --- a/rosplane/src/path_manager_example.cpp +++ b/rosplane/src/path_manager_example.cpp @@ -9,17 +9,17 @@ namespace rosplane { -path_manager_example::path_manager_example() : path_manager_base() +path_manager_example::path_manager_example() + : path_manager_base() { fil_state_ = fillet_state::STRAIGHT; dub_state_ = dubin_state::FIRST; } -void path_manager_example::manage(const params_s ¶ms, const input_s &input, output_s &output) +void path_manager_example::manage(const params_s & params, const input_s & input, output_s & output) { - if (num_waypoints_ < 2) - { + if (num_waypoints_ < 2) { //ROS_WARN_THROTTLE(4, "No waypoints received! Loitering about origin at 50m"); //RCLCPP_WARN(this->get_logger(), "No waypoints received! Loitering about origin at 50m",4); !!! output.flag = false; @@ -29,15 +29,10 @@ void path_manager_example::manage(const params_s ¶ms, const input_s &input, output.c[2] = -50.0f; output.rho = params.R_min; output.lamda = 1; - } - else - { - if (waypoints_[idx_a_].chi_valid) - { + } else { + if (waypoints_[idx_a_].chi_valid) { manage_dubins(params, input, output); - } - else - { + } else { /** Switch the following for flying directly to waypoints, or filleting corners */ //manage_line(params, input, output); manage_fillet(params, input, output); @@ -45,7 +40,8 @@ void path_manager_example::manage(const params_s ¶ms, const input_s &input, } } -void path_manager_example::manage_line(const params_s ¶ms, const input_s &input, output_s &output) +void path_manager_example::manage_line(const params_s & params, const input_s & input, + output_s & output) { Eigen::Vector3f p; @@ -53,14 +49,12 @@ void path_manager_example::manage_line(const params_s ¶ms, const input_s &in int idx_b; int idx_c; - if (idx_a_ == num_waypoints_ - 1) - { - - if (params.orbit_last) - { + if (idx_a_ == num_waypoints_ - 1) { + + if (params.orbit_last) { output.flag = false; output.va_d = waypoints_[idx_a_].va_d; - output.c[0] = waypoints_[idx_a_].w[0]; + output.c[0] = waypoints_[idx_a_].w[0]; output.c[1] = waypoints_[idx_a_].w[1]; output.c[2] = waypoints_[idx_a_].w[2]; output.r[0] = 0.0; @@ -71,12 +65,9 @@ void path_manager_example::manage_line(const params_s ¶ms, const input_s &in output.q[2] = 0.0; output.rho = params.R_min; - if (waypoints_[idx_a_].chi_d < M_PI) - { - output.lamda = 1; - } - else - { + if (waypoints_[idx_a_].chi_d < M_PI) { + output.lamda = 1; + } else { output.lamda = -1; } @@ -85,16 +76,13 @@ void path_manager_example::manage_line(const params_s ¶ms, const input_s &in idx_b = 0; idx_c = 1; - } - else if (idx_a_ == num_waypoints_ - 2) - { + } else if (idx_a_ == num_waypoints_ - 2) { idx_b = num_waypoints_ - 1; idx_c = 0; - } - else - { + } else { idx_b = idx_a_ + 1; - idx_c = idx_b + 1; } + idx_c = idx_b + 1; + } Eigen::Vector3f w_im1(waypoints_[idx_a_].w); Eigen::Vector3f w_i(waypoints_[idx_b].w); @@ -112,21 +100,17 @@ void path_manager_example::manage_line(const params_s ¶ms, const input_s &in output.q[2] = q_im1(2); Eigen::Vector3f n_i = (q_im1 + q_i).normalized(); - if ((p - w_i).dot(n_i) > 0.0f) - { - if (idx_a_ == num_waypoints_ - 1) - { + if ((p - w_i).dot(n_i) > 0.0f) { + if (idx_a_ == num_waypoints_ - 1) { idx_a_ = 0; - } - else - { + } else { idx_a_++; } } - } -void path_manager_example::manage_fillet(const params_s ¶ms, const input_s &input, output_s &output) +void path_manager_example::manage_fillet(const params_s & params, const input_s & input, + output_s & output) { if (num_waypoints_ < 3) //since it fillets don't make sense between just two points { @@ -139,14 +123,12 @@ void path_manager_example::manage_fillet(const params_s ¶ms, const input_s & int idx_b; int idx_c; - if (idx_a_ == num_waypoints_ - 1) - { + if (idx_a_ == num_waypoints_ - 1) { - if (params.orbit_last) - { + if (params.orbit_last) { output.flag = false; output.va_d = waypoints_[idx_a_].va_d; - output.c[0] = waypoints_[idx_a_].w[0]; + output.c[0] = waypoints_[idx_a_].w[0]; output.c[1] = waypoints_[idx_a_].w[1]; output.c[2] = waypoints_[idx_a_].w[2]; output.r[0] = 0.0; @@ -157,12 +139,9 @@ void path_manager_example::manage_fillet(const params_s ¶ms, const input_s & output.q[2] = 0.0; output.rho = params.R_min; - if (waypoints_[idx_a_].chi_d < M_PI) - { - output.lamda = 1; - } - else - { + if (waypoints_[idx_a_].chi_d < M_PI) { + output.lamda = 1; + } else { output.lamda = -1; } @@ -171,24 +150,17 @@ void path_manager_example::manage_fillet(const params_s ¶ms, const input_s & idx_b = 0; idx_c = 1; - } - else if (idx_a_ == num_waypoints_ - 2) - { + } else if (idx_a_ == num_waypoints_ - 2) { idx_b = num_waypoints_ - 1; - if (params.orbit_last) - { + if (params.orbit_last) { idx_c = 0; idx_a_ = idx_b; return; - } - else - { + } else { idx_c = 0; } - } - else - { + } else { idx_b = idx_a_ + 1; idx_c = idx_b + 1; } @@ -208,47 +180,44 @@ void path_manager_example::manage_fillet(const params_s ¶ms, const input_s & float beta = acosf(-q_im1.dot(q_i)); Eigen::Vector3f z; - switch (fil_state_) - { - case fillet_state::STRAIGHT: - output.flag = true; - output.q[0] = q_im1(0); - output.q[1] = q_im1(1); - output.q[2] = q_im1(2); - output.c[0] = 1; - output.c[1] = 1; - output.c[2] = 1; - output.rho = 1; - output.lamda = 1; - z = w_i - q_im1*(R_min/tanf(beta/2.0)); - if ((p - z).dot(q_im1) > 0) - fil_state_ = fillet_state::ORBIT; - break; - case fillet_state::ORBIT: - output.flag = false; - output.q[0] = q_i(0); - 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(beta/2.0)); - output.c[0] = c(0); - output.c[1] = c(1); - output.c[2] = c(2); - output.rho = R_min; - output.lamda = ((q_im1(0)*q_i(1) - q_im1(1)*q_i(0)) > 0 ? 1 : -1); - z = w_i + q_i*(R_min/tanf(beta/2.0)); - if ((p - z).dot(q_i) > 0) - { - if (idx_a_ == num_waypoints_ - 1) - idx_a_ = 0; - else - idx_a_++; - fil_state_ = fillet_state::STRAIGHT; - } - break; + switch (fil_state_) { + case fillet_state::STRAIGHT: + output.flag = true; + output.q[0] = q_im1(0); + output.q[1] = q_im1(1); + output.q[2] = q_im1(2); + output.c[0] = 1; + output.c[1] = 1; + output.c[2] = 1; + output.rho = 1; + output.lamda = 1; + z = w_i - q_im1 * (R_min / tanf(beta / 2.0)); + if ((p - z).dot(q_im1) > 0) fil_state_ = fillet_state::ORBIT; + break; + case fillet_state::ORBIT: + output.flag = false; + output.q[0] = q_i(0); + 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(beta / 2.0)); + output.c[0] = c(0); + output.c[1] = c(1); + output.c[2] = c(2); + output.rho = R_min; + output.lamda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1); + z = w_i + q_i * (R_min / tanf(beta / 2.0)); + if ((p - z).dot(q_i) > 0) { + if (idx_a_ == num_waypoints_ - 1) idx_a_ = 0; + else + idx_a_++; + fil_state_ = fillet_state::STRAIGHT; + } + break; } } -void path_manager_example::manage_dubins(const params_s ¶ms, const input_s &input, output_s &output) +void path_manager_example::manage_dubins(const params_s & params, const input_s & input, + output_s & output) { Eigen::Vector3f p; p << input.pn, input.pe, -input.h; @@ -264,136 +233,122 @@ void path_manager_example::manage_dubins(const params_s ¶ms, const input_s & output.c[1] = 0; output.c[2] = 0; - switch (dub_state_) - { - case dubin_state::FIRST: - dubinsParameters(waypoints_[0], waypoints_[1], params.R_min); - output.flag = false; - output.c[0] = dubinspath_.cs(0); - output.c[1] = dubinspath_.cs(1); - output.c[2] = dubinspath_.cs(2); - output.rho = dubinspath_.R; - output.lamda = dubinspath_.lams; - if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1 - { - dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE; - } - else - { - dub_state_ = dubin_state::BEFORE_H1; - } - break; - case dubin_state::BEFORE_H1: - output.flag = false; - output.c[0] = dubinspath_.cs(0); - output.c[1] = dubinspath_.cs(1); - output.c[2] = dubinspath_.cs(2); - output.rho = dubinspath_.R; - output.lamda = dubinspath_.lams; - if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // entering H1 - { - dub_state_ = dubin_state::STRAIGHT; - } - break; - case dubin_state::BEFORE_H1_WRONG_SIDE: - output.flag = false; - output.c[0] = dubinspath_.cs(0); - output.c[1] = dubinspath_.cs(1); - output.c[2] = dubinspath_.cs(2); - output.rho = dubinspath_.R; - output.lamda = dubinspath_.lams; - if ((p - dubinspath_.w1).dot(dubinspath_.q1) < 0) // exit H1 - { - dub_state_ = dubin_state::BEFORE_H1; - } - break; - case dubin_state::STRAIGHT: - output.flag = true; - output.r[0] = dubinspath_.w1(0); - output.r[1] = dubinspath_.w1(1); - output.r[2] = dubinspath_.w1(2); - // output.r[0] = dubinspath_.z1(0); - // output.r[1] = dubinspath_.z1(1); - // output.r[2] = dubinspath_.z1(2); - output.q[0] = dubinspath_.q1(0); - output.q[1] = dubinspath_.q1(1); - output.q[2] = dubinspath_.q1(2); - output.rho = 1; - output.lamda = 1; - if ((p - dubinspath_.w2).dot(dubinspath_.q1) >= 0) // entering H2 - { - if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // start in H3 - { - dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE; - } - else + switch (dub_state_) { + case dubin_state::FIRST: + dubinsParameters(waypoints_[0], waypoints_[1], params.R_min); + output.flag = false; + output.c[0] = dubinspath_.cs(0); + output.c[1] = dubinspath_.cs(1); + output.c[2] = dubinspath_.cs(2); + output.rho = dubinspath_.R; + output.lamda = dubinspath_.lams; + if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1 { - dub_state_ = dubin_state::BEFORE_H3; + dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE; + } else { + dub_state_ = dubin_state::BEFORE_H1; } - } - break; - case dubin_state::BEFORE_H3: - output.flag = false; - output.c[0] = dubinspath_.ce(0); - output.c[1] = dubinspath_.ce(1); - output.c[2] = dubinspath_.ce(2); - output.rho = dubinspath_.R; - output.lamda = dubinspath_.lame; - if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // entering H3 - { - // increase the waypoint pointer - int idx_b; - if (idx_a_ == num_waypoints_ - 1) + break; + case dubin_state::BEFORE_H1: + output.flag = false; + output.c[0] = dubinspath_.cs(0); + output.c[1] = dubinspath_.cs(1); + output.c[2] = dubinspath_.cs(2); + output.rho = dubinspath_.R; + output.lamda = dubinspath_.lams; + if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // entering H1 { - idx_a_ = 0; - idx_b = 1; + dub_state_ = dubin_state::STRAIGHT; } - else if (idx_a_ == num_waypoints_ - 2) + break; + case dubin_state::BEFORE_H1_WRONG_SIDE: + output.flag = false; + output.c[0] = dubinspath_.cs(0); + output.c[1] = dubinspath_.cs(1); + output.c[2] = dubinspath_.cs(2); + output.rho = dubinspath_.R; + output.lamda = dubinspath_.lams; + if ((p - dubinspath_.w1).dot(dubinspath_.q1) < 0) // exit H1 { - idx_a_++; - idx_b = 0; + dub_state_ = dubin_state::BEFORE_H1; } - else + break; + case dubin_state::STRAIGHT: + output.flag = true; + output.r[0] = dubinspath_.w1(0); + output.r[1] = dubinspath_.w1(1); + output.r[2] = dubinspath_.w1(2); + // output.r[0] = dubinspath_.z1(0); + // output.r[1] = dubinspath_.z1(1); + // output.r[2] = dubinspath_.z1(2); + output.q[0] = dubinspath_.q1(0); + output.q[1] = dubinspath_.q1(1); + output.q[2] = dubinspath_.q1(2); + output.rho = 1; + output.lamda = 1; + if ((p - dubinspath_.w2).dot(dubinspath_.q1) >= 0) // entering H2 { - idx_a_++; - idx_b = idx_a_ + 1; + if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // start in H3 + { + dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE; + } else { + dub_state_ = dubin_state::BEFORE_H3; + } } - - // plan new Dubin's path to next waypoint configuration - dubinsParameters(waypoints_[idx_a_], waypoints_[idx_b], params.R_min); - - //start new path - if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1 + break; + case dubin_state::BEFORE_H3: + output.flag = false; + output.c[0] = dubinspath_.ce(0); + output.c[1] = dubinspath_.ce(1); + output.c[2] = dubinspath_.ce(2); + output.rho = dubinspath_.R; + output.lamda = dubinspath_.lame; + if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // entering H3 { - dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE; + // increase the waypoint pointer + int idx_b; + if (idx_a_ == num_waypoints_ - 1) { + idx_a_ = 0; + idx_b = 1; + } else if (idx_a_ == num_waypoints_ - 2) { + idx_a_++; + idx_b = 0; + } else { + idx_a_++; + idx_b = idx_a_ + 1; + } + + // plan new Dubin's path to next waypoint configuration + dubinsParameters(waypoints_[idx_a_], waypoints_[idx_b], params.R_min); + + //start new path + if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1 + { + dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE; + } else { + dub_state_ = dubin_state::BEFORE_H1; + } } - else + break; + case dubin_state::BEFORE_H3_WRONG_SIDE: + output.flag = false; + output.c[0] = dubinspath_.ce(0); + output.c[1] = dubinspath_.ce(1); + output.c[2] = dubinspath_.ce(2); + output.rho = dubinspath_.R; + output.lamda = dubinspath_.lame; + if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3 { dub_state_ = dubin_state::BEFORE_H1; } - } - break; - case dubin_state::BEFORE_H3_WRONG_SIDE: - output.flag = false; - output.c[0] = dubinspath_.ce(0); - output.c[1] = dubinspath_.ce(1); - output.c[2] = dubinspath_.ce(2); - output.rho = dubinspath_.R; - output.lamda = dubinspath_.lame; - if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3 - { - dub_state_ = dubin_state::BEFORE_H1; - } - break; + break; } } Eigen::Matrix3f path_manager_example::rotz(float theta) { Eigen::Matrix3f R; - R << cosf(theta), -sinf(theta), 0, - sinf(theta), cosf(theta), 0, - 0, 0, 1; + R << cosf(theta), -sinf(theta), 0, sinf(theta), cosf(theta), 0, 0, 0, 1; return R; } @@ -401,28 +356,24 @@ Eigen::Matrix3f path_manager_example::rotz(float theta) float path_manager_example::mo(float in) { float val; - 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; + 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; } return val; } -void path_manager_example::dubinsParameters(const waypoint_s start_node, const waypoint_s end_node, float R) +void path_manager_example::dubinsParameters(const waypoint_s start_node, const waypoint_s end_node, + 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])); - if (ell < 2.0*R) - { - //ROS_ERROR("The distance between nodes must be larger than 2R."); - RCLCPP_ERROR(this->get_logger(),"The distance between nodes must be larger than 2R."); + 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])); + if (ell < 2.0 * R) { + //ROS_ERROR("The distance between nodes must be larger than 2R."); + RCLCPP_ERROR(this->get_logger(), "The distance between nodes must be larger than 2R."); - } - else - { + } else { dubinspath_.ps(0) = start_node.w[0]; dubinspath_.ps(1) = start_node.w[1]; dubinspath_.ps(2) = start_node.w[2]; @@ -432,72 +383,76 @@ void path_manager_example::dubinsParameters(const waypoint_s start_node, const w dubinspath_.pe(2) = end_node.w[2]; dubinspath_.chie = end_node.chi_d; - Eigen::Vector3f crs = dubinspath_.ps; - crs(0) += R*(cosf(M_PI_2_F)*cosf(dubinspath_.chis) - sinf(M_PI_2_F)*sinf(dubinspath_.chis)); - crs(1) += R*(sinf(M_PI_2_F)*cosf(dubinspath_.chis) + cosf(M_PI_2_F)*sinf(dubinspath_.chis)); + crs(0) += + R * (cosf(M_PI_2_F) * cosf(dubinspath_.chis) - sinf(M_PI_2_F) * sinf(dubinspath_.chis)); + crs(1) += + R * (sinf(M_PI_2_F) * cosf(dubinspath_.chis) + cosf(M_PI_2_F) * sinf(dubinspath_.chis)); Eigen::Vector3f cls = dubinspath_.ps; - cls(0) += R*(cosf(-M_PI_2_F)*cosf(dubinspath_.chis) - sinf(-M_PI_2_F)*sinf(dubinspath_.chis)); - cls(1) += R*(sinf(-M_PI_2_F)*cosf(dubinspath_.chis) + cosf(-M_PI_2_F)*sinf(dubinspath_.chis)); + cls(0) += + R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chis) - sinf(-M_PI_2_F) * sinf(dubinspath_.chis)); + cls(1) += + R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chis) + cosf(-M_PI_2_F) * sinf(dubinspath_.chis)); Eigen::Vector3f cre = dubinspath_.pe; - cre(0) += R*(cosf(M_PI_2_F)*cosf(dubinspath_.chie) - sinf(M_PI_2_F)*sinf(dubinspath_.chie)); - cre(1) += R*(sinf(M_PI_2_F)*cosf(dubinspath_.chie) + cosf(M_PI_2_F)*sinf(dubinspath_.chie)); + cre(0) += + R * (cosf(M_PI_2_F) * cosf(dubinspath_.chie) - sinf(M_PI_2_F) * sinf(dubinspath_.chie)); + cre(1) += + R * (sinf(M_PI_2_F) * cosf(dubinspath_.chie) + cosf(M_PI_2_F) * sinf(dubinspath_.chie)); Eigen::Vector3f cle = dubinspath_.pe; - cle(0) += R*(cosf(-M_PI_2_F)*cosf(dubinspath_.chie) - sinf(-M_PI_2_F)*sinf(dubinspath_.chie)); - cle(1) += R*(sinf(-M_PI_2_F)*cosf(dubinspath_.chie) + cosf(-M_PI_2_F)*sinf(dubinspath_.chie)); + cle(0) += + R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chie) - sinf(-M_PI_2_F) * sinf(dubinspath_.chie)); + cle(1) += + R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chie) + cosf(-M_PI_2_F) * sinf(dubinspath_.chie)); float theta, theta2; // compute L1 theta = atan2f(cre(1) - crs(1), cre(0) - crs(0)); - float L1 = (crs - cre).norm() + R*mo(2.0*M_PI_F + mo(theta - M_PI_2_F) - mo(dubinspath_.chis - M_PI_2_F)) - + R*mo(2.0*M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta - M_PI_2_F)); + float L1 = (crs - cre).norm() + + R * mo(2.0 * M_PI_F + mo(theta - M_PI_2_F) - mo(dubinspath_.chis - M_PI_2_F)) + + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta - M_PI_2_F)); // compute L2 ell = (cle - crs).norm(); theta = atan2f(cle(1) - crs(1), cle(0) - crs(0)); float L2; - 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) + R*mo(2.0*M_PI_F + mo(theta2) - mo(dubinspath_.chis - M_PI_2_F)) - + R*mo(2.0*M_PI_F + mo(theta2 + M_PI_F) - mo(dubinspath_.chie + M_PI_2_F)); + 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) + + R * mo(2.0 * M_PI_F + mo(theta2) - mo(dubinspath_.chis - M_PI_2_F)) + + R * mo(2.0 * M_PI_F + mo(theta2 + M_PI_F) - mo(dubinspath_.chie + M_PI_2_F)); } // compute L3 ell = (cre - cls).norm(); theta = atan2f(cre(1) - cls(1), cre(0) - cls(0)); float L3; - if (2.0*R > ell) - L3 = 9999.0f; - else - { - theta2 = acosf(2.0*R/ell); - L3 = sqrtf(ell*ell - 4*R*R) + R*mo(2.0*M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + theta2)) - + R*mo(2.0*M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta + theta2 - M_PI_F)); + if (2.0 * R > ell) L3 = 9999.0f; + else { + theta2 = acosf(2.0 * R / ell); + L3 = sqrtf(ell * ell - 4 * R * R) + + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + theta2)) + + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta + theta2 - M_PI_F)); } // compute L4 theta = atan2f(cle(1) - cls(1), cle(0) - cls(0)); - float L4 = (cls - cle).norm() + R*mo(2.0*M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + M_PI_2_F)) - + R*mo(2.0*M_PI_F + mo(theta + M_PI_2_F) - mo(dubinspath_.chie + M_PI_2_F)); + float L4 = (cls - cle).norm() + + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + M_PI_2_F)) + + R * mo(2.0 * M_PI_F + mo(theta + M_PI_2_F) - mo(dubinspath_.chie + M_PI_2_F)); // L is the minimum distance int idx = 1; dubinspath_.L = L1; - if (L2 < dubinspath_.L) - { + if (L2 < dubinspath_.L) { dubinspath_.L = L2; idx = 2; } - if (L3 < dubinspath_.L) - { + if (L3 < dubinspath_.L) { dubinspath_.L = L3; idx = 3; } - if (L4 < dubinspath_.L) - { + if (L4 < dubinspath_.L) { dubinspath_.L = L4; idx = 4; } @@ -507,55 +462,54 @@ void path_manager_example::dubinsParameters(const waypoint_s start_node, const w e1(0) = 1; e1(1) = 0; e1(2) = 0; - switch (idx) - { - case 1: - dubinspath_.cs = crs; - dubinspath_.lams = 1; - dubinspath_.ce = cre; - dubinspath_.lame = 1; - dubinspath_.q1 = (cre - crs).normalized(); - dubinspath_.w1 = dubinspath_.cs + (rotz(-M_PI_2_F)*dubinspath_.q1)*R; - dubinspath_.w2 = dubinspath_.ce + (rotz(-M_PI_2_F)*dubinspath_.q1)*R; - break; - case 2: - dubinspath_.cs = crs; - dubinspath_.lams = 1; - dubinspath_.ce = cle; - dubinspath_.lame = -1; - ell = (cle - crs).norm(); - theta = atan2f(cle(1) - crs(1), cle(0) - crs(0)); - theta2 = theta - M_PI_2_F + asinf(2.0*R/ell); - dubinspath_.q1 = rotz(theta2 + M_PI_2_F)*e1; - dubinspath_.w1 = dubinspath_.cs + (rotz(theta2)*e1)*R; - dubinspath_.w2 = dubinspath_.ce + (rotz(theta2 + M_PI_F)*e1)*R; - break; - case 3: - dubinspath_.cs = cls; - dubinspath_.lams = -1; - dubinspath_.ce = cre; - dubinspath_.lame = 1; - ell = (cre - cls).norm(); - theta = atan2f(cre(1) - cls(1), cre(0) - cls(0)); - theta2 = acosf(2.0*R/ ell); - dubinspath_.q1 = rotz(theta + theta2 - M_PI_2_F)*e1; - dubinspath_.w1 = dubinspath_.cs + (rotz(theta + theta2)*e1)*R; - dubinspath_.w2 = dubinspath_.ce + (rotz(theta + theta2 - M_PI_F)*e1)*R; - break; - case 4: - dubinspath_.cs = cls; - dubinspath_.lams = -1; - dubinspath_.ce = cle; - dubinspath_.lame = -1; - dubinspath_.q1 = (cle - cls).normalized(); - dubinspath_.w1 = dubinspath_.cs + (rotz(M_PI_2_F)*dubinspath_.q1)*R; - dubinspath_.w2 = dubinspath_.ce + (rotz(M_PI_2_F)*dubinspath_.q1)*R; - break; + switch (idx) { + case 1: + dubinspath_.cs = crs; + dubinspath_.lams = 1; + dubinspath_.ce = cre; + dubinspath_.lame = 1; + dubinspath_.q1 = (cre - crs).normalized(); + dubinspath_.w1 = dubinspath_.cs + (rotz(-M_PI_2_F) * dubinspath_.q1) * R; + dubinspath_.w2 = dubinspath_.ce + (rotz(-M_PI_2_F) * dubinspath_.q1) * R; + break; + case 2: + dubinspath_.cs = crs; + dubinspath_.lams = 1; + dubinspath_.ce = cle; + dubinspath_.lame = -1; + ell = (cle - crs).norm(); + theta = atan2f(cle(1) - crs(1), cle(0) - crs(0)); + theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell); + dubinspath_.q1 = rotz(theta2 + M_PI_2_F) * e1; + dubinspath_.w1 = dubinspath_.cs + (rotz(theta2) * e1) * R; + dubinspath_.w2 = dubinspath_.ce + (rotz(theta2 + M_PI_F) * e1) * R; + break; + case 3: + dubinspath_.cs = cls; + dubinspath_.lams = -1; + dubinspath_.ce = cre; + dubinspath_.lame = 1; + ell = (cre - cls).norm(); + theta = atan2f(cre(1) - cls(1), cre(0) - cls(0)); + theta2 = acosf(2.0 * R / ell); + dubinspath_.q1 = rotz(theta + theta2 - M_PI_2_F) * e1; + dubinspath_.w1 = dubinspath_.cs + (rotz(theta + theta2) * e1) * R; + dubinspath_.w2 = dubinspath_.ce + (rotz(theta + theta2 - M_PI_F) * e1) * R; + break; + case 4: + dubinspath_.cs = cls; + dubinspath_.lams = -1; + dubinspath_.ce = cle; + dubinspath_.lame = -1; + dubinspath_.q1 = (cle - cls).normalized(); + dubinspath_.w1 = dubinspath_.cs + (rotz(M_PI_2_F) * dubinspath_.q1) * R; + dubinspath_.w2 = dubinspath_.ce + (rotz(M_PI_2_F) * dubinspath_.q1) * R; + break; } dubinspath_.w3 = dubinspath_.pe; - dubinspath_.q3 = rotz(dubinspath_.chie)*e1; + dubinspath_.q3 = rotz(dubinspath_.chie) * e1; dubinspath_.R = R; } } -}//end namespace +} // namespace rosplane diff --git a/rosplane/src/path_planner.cpp b/rosplane/src/path_planner.cpp index 7042bf5..42dba47 100644 --- a/rosplane/src/path_planner.cpp +++ b/rosplane/src/path_planner.cpp @@ -13,8 +13,9 @@ using std::placeholders::_1; using namespace std::chrono_literals; -namespace rosplane { - +namespace rosplane +{ + class path_planner : public rclcpp::Node { public: @@ -26,59 +27,52 @@ class path_planner : public rclcpp::Node rclcpp::Service::SharedPtr waypoint_service; rclcpp::TimerBase::SharedPtr timer_; - bool publish_next_waypoint( - const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res); - + bool publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res); + void waypoint_publish(); - + int num_waypoints_published; - float wps[5*num_waypoints]; - + float wps[5 * num_waypoints]; }; -path_planner::path_planner() : Node("path_planner") { +path_planner::path_planner() + : Node("path_planner") +{ waypoint_publisher = this->create_publisher("waypoint_path", 10); - waypoint_service = this->create_service("publish_next_waypoint", std::bind(&path_planner::publish_next_waypoint, this, std::placeholders::_1, std::placeholders::_2)); - + waypoint_service = this->create_service( + "publish_next_waypoint", + std::bind(&path_planner::publish_next_waypoint, this, std::placeholders::_1, + std::placeholders::_2)); + timer_ = this->create_wall_timer(1000ms, std::bind(&path_planner::waypoint_publish, this)); num_waypoints_published = 0; float va = 25; - float wps_to_add[5*num_waypoints] = - { - 0, 100, -90, 45*M_PI/180, va, - 0, 300, -90, 45*M_PI/180, va, - 0, 500, -90, 45*M_PI/180, va, - 0, 700, -90, 45*M_PI/180, va, - 0, 900, -90, 45*M_PI/180, va, - 0, 1100, -90, 45*M_PI/180, va, - 0, 1300, -90, 45*M_PI/180, va - }; - - for (int i = 0; i < 5*num_waypoints; i++) - { - wps[i] = wps_to_add[i]; - } + float wps_to_add[5 * num_waypoints] = { + 0, 100, -90, 45 * M_PI / 180, va, 0, 300, -90, 45 * M_PI / 180, va, + 0, 500, -90, 45 * M_PI / 180, va, 0, 700, -90, 45 * M_PI / 180, va, + 0, 900, -90, 45 * M_PI / 180, va, 0, 1100, -90, 45 * M_PI / 180, va, + 0, 1300, -90, 45 * M_PI / 180, va}; + for (int i = 0; i < 5 * num_waypoints; i++) { wps[i] = wps_to_add[i]; } } -path_planner::~path_planner() { -} +path_planner::~path_planner() {} -bool path_planner::publish_next_waypoint( - const std_srvs::srv::Trigger::Request::SharedPtr & req, - const std_srvs::srv::Trigger::Response::SharedPtr & res) +bool path_planner::publish_next_waypoint(const std_srvs::srv::Trigger::Request::SharedPtr & req, + const std_srvs::srv::Trigger::Response::SharedPtr & res) { - RCLCPP_INFO_STREAM(this->get_logger(), "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published); + RCLCPP_INFO_STREAM( + this->get_logger(), + "Publishing next waypoint, num_waypoints_published: " << num_waypoints_published); - if (num_waypoints_published < num_waypoints) - { + if (num_waypoints_published < num_waypoints) { rosplane_msgs::msg::Waypoint new_waypoint; rclcpp::Time now = this->get_clock()->now(); @@ -87,15 +81,14 @@ bool path_planner::publish_next_waypoint( int i = num_waypoints_published; - new_waypoint.w[0] = wps[i*5 + 0]; - new_waypoint.w[1] = wps[i*5 + 1]; - new_waypoint.w[2] = wps[i*5 + 2]; - new_waypoint.chi_d = wps[i*5 + 3]; + new_waypoint.w[0] = wps[i * 5 + 0]; + new_waypoint.w[1] = wps[i * 5 + 1]; + new_waypoint.w[2] = wps[i * 5 + 2]; + new_waypoint.chi_d = wps[i * 5 + 3]; new_waypoint.chi_valid = false; - new_waypoint.va_d = wps[i*5 + 4]; - if (i == 0) - new_waypoint.set_current = true; + new_waypoint.va_d = wps[i * 5 + 4]; + if (i == 0) new_waypoint.set_current = true; else new_waypoint.set_current = false; new_waypoint.clear_wp_list = false; @@ -103,7 +96,6 @@ bool path_planner::publish_next_waypoint( waypoint_publisher->publish(new_waypoint); num_waypoints_published++; - } return true; @@ -112,9 +104,8 @@ bool path_planner::publish_next_waypoint( void path_planner::waypoint_publish() { - if (num_waypoints_published < num_waypoints_to_publish) - { - + if (num_waypoints_published < num_waypoints_to_publish) { + rosplane_msgs::msg::Waypoint new_waypoint; rclcpp::Time now = this->get_clock()->now(); @@ -123,15 +114,14 @@ void path_planner::waypoint_publish() int i = num_waypoints_published; - new_waypoint.w[0] = wps[i*5 + 0]; - new_waypoint.w[1] = wps[i*5 + 1]; - new_waypoint.w[2] = wps[i*5 + 2]; - new_waypoint.chi_d = wps[i*5 + 3]; + new_waypoint.w[0] = wps[i * 5 + 0]; + new_waypoint.w[1] = wps[i * 5 + 1]; + new_waypoint.w[2] = wps[i * 5 + 2]; + new_waypoint.chi_d = wps[i * 5 + 3]; new_waypoint.chi_valid = false; - new_waypoint.va_d = wps[i*5 + 4]; - if (i == 0) - new_waypoint.set_current = true; + new_waypoint.va_d = wps[i * 5 + 4]; + if (i == 0) new_waypoint.set_current = true; else new_waypoint.set_current = false; new_waypoint.clear_wp_list = false; @@ -139,14 +129,11 @@ void path_planner::waypoint_publish() waypoint_publisher->publish(new_waypoint); num_waypoints_published++; - } - - -} } +} // namespace rosplane -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/rosplane_sim/src/gazebo_state_transcription.cpp b/rosplane_sim/src/gazebo_state_transcription.cpp index 32ca8ac..ecd7280 100644 --- a/rosplane_sim/src/gazebo_state_transcription.cpp +++ b/rosplane_sim/src/gazebo_state_transcription.cpp @@ -1,31 +1,33 @@ #include -#include "rclcpp/rclcpp.hpp" -#include "nav_msgs/msg/odometry.hpp" +#include "Eigen/Geometry" #include "geometry_msgs/msg/vector3_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" #include "rosplane_msgs/msg/state.hpp" -#include "Eigen/Geometry" #include "chrono" using namespace std::chrono_literals; -using std::placeholders::_1; using rosplane_msgs::msg::State; +using std::placeholders::_1; class gazebo_transcription : public rclcpp::Node { public: gazebo_transcription() - : Node("gazebo_state") + : Node("gazebo_state") { - odom_subscription_ = this->create_subscription("/fixedwing/truth/NED", 10, std::bind(&gazebo_transcription::publish_truth, this, _1)); + odom_subscription_ = this->create_subscription( + "/fixedwing/truth/NED", 10, std::bind(&gazebo_transcription::publish_truth, this, _1)); publisher_ = this->create_publisher("state", 10); } private: rclcpp::Subscription::SharedPtr odom_subscription_; - rclcpp::Subscription::SharedPtr euler_sub_; // TODO is there going to be gitter between the times each of these collected? + rclcpp::Subscription::SharedPtr + euler_sub_; // TODO is there going to be gitter between the times each of these collected? rclcpp::Publisher::SharedPtr publisher_; @@ -34,9 +36,8 @@ class gazebo_transcription : public rclcpp::Node double we = 0.0; double wd = 0.0; - - - void publish_truth(const nav_msgs::msg::Odometry &msg){ + void publish_truth(const nav_msgs::msg::Odometry & msg) + { rosplane_msgs::msg::State state; @@ -58,9 +59,11 @@ class gazebo_transcription : public rclcpp::Node q.z() = msg.pose.pose.orientation.z; Eigen::Vector3f euler; - euler(0) = atan2(2.0*(q.w()*q.x() + q.y()*q.z()), pow(q.w(), 2) + pow(q.z(),2) - pow(q.x(), 2) - pow(q.y(), 2)); - euler(1) = asin(2.0* (q.w()*q.y() - q.x()*q.z())); - euler(2) = atan2(2.0*(q.w()*q.z() + q.x()*q.y()), pow(q.w(), 2) + pow(q.x(), 2) - pow(q.y(), 2) - pow(q.z(), 2)); + euler(0) = atan2(2.0 * (q.w() * q.x() + q.y() * q.z()), + pow(q.w(), 2) + pow(q.z(), 2) - pow(q.x(), 2) - pow(q.y(), 2)); + euler(1) = asin(2.0 * (q.w() * q.y() - q.x() * q.z())); + euler(2) = atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), + pow(q.w(), 2) + pow(q.x(), 2) - pow(q.y(), 2) - pow(q.z(), 2)); state.phi = euler(0); state.theta = euler(1); @@ -89,9 +92,9 @@ class gazebo_transcription : public rclcpp::Node state.va = std::sqrt(pow(ur, 2) + pow(vr, 2) + pow(wr, 2)); - state.chi = atan2(state.va*sin(state.psi), state.va*cos(state.psi)); + state.chi = atan2(state.va * sin(state.psi), state.va * cos(state.psi)); state.alpha = atan2(wr, ur); - state.beta = asin(vr/state.va); + state.beta = asin(vr / state.va); state.quat_valid = true; @@ -100,18 +103,15 @@ class gazebo_transcription : public rclcpp::Node state.quat[2] = msg.pose.pose.orientation.y; state.quat[3] = msg.pose.pose.orientation.z; -// state.psi_deg = state.psi * TODO implement the deg into the state. + // state.psi_deg = state.psi * TODO implement the deg into the state. publisher_->publish(state); - } - - }; int main(int argc, char * argv[]) { - rclcpp:: init(argc, argv); + rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0;