├── .astylerc ├── .gitignore ├── README.md ├── fix_code_style.sh ├── rosplane ├── CMakeLists.txt ├── cfg │ ├── Controller.cfg │ └── Follower.cfg ├── include │ ├── controller_base.h │ ├── controller_example.h │ ├── estimator_base.h │ ├── estimator_example.h │ ├── path_follower_base.h │ ├── path_follower_example.h │ ├── path_manager_base.h │ └── path_manager_example.h ├── package.xml ├── param │ ├── aerosonde.yaml │ └── bixler.yaml └── src │ ├── controller_base.cpp │ ├── controller_example.cpp │ ├── estimator_base.cpp │ ├── estimator_example.cpp │ ├── path_follower_base.cpp │ ├── path_follower_example.cpp │ ├── path_manager_base.cpp │ ├── path_manager_example.cpp │ └── path_planner.cpp ├── rosplane_msgs ├── CMakeLists.txt ├── msg │ ├── Controller_Commands.msg │ ├── Controller_Internals.msg │ ├── Current_Path.msg │ ├── State.msg │ └── Waypoint.msg └── package.xml └── rosplane_sim ├── CMakeLists.txt ├── include └── rosplane_sim │ ├── aircraft_forces_and_moments.h │ ├── aircraft_truth.h │ ├── common.h │ └── gz_compat.h ├── launch ├── fixedwing.launch ├── rosflight_sil.launch └── spawn_mav.launch ├── package.xml ├── params └── fixedwing.yaml ├── src ├── aircraft_forces_and_moments.cpp └── aircraft_truth.cpp └── xacro ├── aircraft_forces_and_moments.xacro ├── aircraft_truth.xacro ├── fixedwing.xacro ├── fixedwing_sil.xacro └── meshes ├── byu_wing.png └── fixedwing.dae /.astylerc: -------------------------------------------------------------------------------- 1 | style=break 2 | indent=spaces=2 3 | pad-header 4 | unpad-paren 5 | align-pointer=name 6 | align-reference=name 7 | max-code-length=120 8 | suffix=none 9 | pad-comma 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROSplane 2 | 3 | ## ROS 2 Transisiton 4 | 5 | This verison of ROSplane is no longer under development and no longer supported. 6 | ROSplane for ROS2 is currently under development and in beta, you can find ROSplane [here](https://github.com/rosflight/rosplane). 7 | 8 | ## ROSplane for ROS 1 9 | 10 | ROSplane is limited-feature fixed-wing autopilot built around ROS. It is intended to be flown with [ROSflight](https://rosflight.org) as the hardware I/O or in the Gazebo simulation environment. It is built according to the method published in [Small Unmanned Aircraft: Theory and Practice](http://uavbook.byu.edu/doku.php) by Beard and McLain, so as to allow anyone to easily understand, modify and use the code. This framework is inherently modular and extensively documented so as to aid the user in understanding and extending for personal use. 11 | 12 | This repository features three ROS packages: rosplane, rosplane\_msgs, and rosplane\_sim. The contents of each of these three packages are described below. 13 | 14 | To fly in hardware, ROSplane is intended to be used with [ROSflight](https://github.com/rosflight/rosflight) and a flight controller (F1 or F4) running the ROSflight [firmware](https://github.com/rosflight/firmware). 15 | 16 | To fly in simulation, simply build these packages in a catkin workspace and launch fixedwing.launch: 17 | 18 | `$ roslaunch rosplane_sim fixedwing.launch` 19 | 20 | Note: To successfully build, it may be needed to clone [rosflight_plugins](https://github.com/byu-magicc/rosflight_plugins.git) and [ROSflight](https://github.com/rosflight/rosflight.git) into your catkin workspace. Additionally, retrieve the necessary ROSflight submodules with: 21 | 22 | `cd rosflight/` 23 | 24 | `git submodule update --init --recursive` 25 | 26 | 27 | # rosplane 28 | 29 | rosplane contains the principal nodes behind the ROSplane autopilot. Each node is separated into a \_base.h/.cpp and a \_example.h/.cpp in an attempt to abstract the ROS code from the guidance and control code covered in [Small Unmanned Aircraft: Theory and Practice](http://uavbook.byu.edu/doku.php). Each node is described below. 30 | 31 | ## - Estimator 32 | 33 | The estimator is a standard extended Kalman Filter (EKF), implemented as defined in the above reference. It has an attitude filter and a position filter for gps smoothing. We are estimating position, velocity, and attitude. The state is then published in the rosplane_msgs/msg/State.msg. 34 | 35 | ## - Controller 36 | 37 | Implements a nested PID controller according to the reference above. Requires State and Controller_Commands messages to be published. Altitude is controlled in a longitudinal state machine including take-off, climb, desend, and hold zones. Controller can be tuned using rqt_reconfigure. 38 | 39 | ## - Path Follower 40 | 41 | Gets the aircraft onto a Current_Path using vector fields. Chi_inf along with K_path and K_orbit gains can also be tuned using rqt_reconfigure. 42 | 43 | ## - Path Manager 44 | 45 | Receives Waypoint messages and creates a path (straight line, fillet, or Dubins) to acheive all the waypoints. 46 | 47 | # rosplane_msgs 48 | 49 | rosplane_msgs is a ROS package containing the custom message types for ROSplane. These message types include Controller_Commands, Controller_internals, Current_Path, State, and Waypoint. 50 | 51 | 52 | # rosplane_sim 53 | 54 | rosplane_sim contains all the necessary plugins to fly the ROSplane autopilot in the Gazebo simulation. Fixedwing.launch launches a basic simulation with the ROSplane autopilot. rosflight_sil.launch launches a software-in-the-loop simulation for a fixedwing running the ROSflight firmware. An example .yaml file is also included for the simulated airframe. 55 | 56 | If you use this work in your research, please cite: 57 | ``` 58 | @INPROCEEDINGS{ellingson2017rosplane, 59 | author = {Ellingson, Gary and McLain, Tim}, 60 | title = {ROSplane: Fixed-wing Autopilot for Education and Research}, 61 | booktitle = {Unmanned Aircraft Systems (ICUAS), 2017 International Conference on}, 62 | year = {2017} 63 | organization={IEEE} 64 | } 65 | ``` 66 | -------------------------------------------------------------------------------- /fix_code_style.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CMD="astyle --options=.astylerc" 4 | 5 | $CMD rosplane/src/* 6 | $CMD rosplane/include/* 7 | $CMD rosplane_sim/src/* 8 | $CMD rosplane_sim/include/rosplane_sim/* 9 | -------------------------------------------------------------------------------- /rosplane/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosplane) 3 | 4 | set(CMAKE_CXX_STANDARD 11) 5 | if (NOT CMAKE_BUILD_TYPE) 6 | # Options: Debug, Release, MinSizeRel, RelWithDebInfo 7 | message(STATUS "No build type selected, default to Release") 8 | set(CMAKE_BUILD_TYPE "Release") 9 | endif() 10 | 11 | set(CMAKE_CXX_FLAGS "-fopenmp") 12 | 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | rospy 16 | rosflight_msgs 17 | rosplane_msgs 18 | dynamic_reconfigure 19 | sensor_msgs 20 | ) 21 | find_package(Eigen3 REQUIRED) 22 | 23 | ################################################ 24 | ## Declare ROS dynamic reconfigure parameters ## 25 | ################################################ 26 | 27 | ## To declare and build dynamic reconfigure parameters within this 28 | ## package, follow these steps: 29 | ## * In the file package.xml: 30 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 31 | ## * In this file (CMakeLists.txt): 32 | ## * add "dynamic_reconfigure" to 33 | ## find_package(catkin REQUIRED COMPONENTS ...) 34 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 35 | ## and list every .cfg file to be processed 36 | 37 | # Generate dynamic reconfigure parameters in the 'cfg' folder 38 | generate_dynamic_reconfigure_options( 39 | cfg/Controller.cfg 40 | cfg/Follower.cfg 41 | ) 42 | 43 | catkin_package( 44 | INCLUDE_DIRS include 45 | # LIBRARIES 46 | CATKIN_DEPENDS roscpp rospy rosflight_msgs rosplane_msgs 47 | # DEPENDS system_lib 48 | ) 49 | 50 | include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) 51 | 52 | ## Declare a C++ executable 53 | add_executable(rosplane_controller 54 | src/controller_base.cpp 55 | src/controller_example.cpp) 56 | add_dependencies(rosplane_controller ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 57 | target_link_libraries(rosplane_controller ${catkin_LIBRARIES}) 58 | 59 | 60 | ## Declare a C++ executable 61 | add_executable(rosplane_estimator 62 | src/estimator_base.cpp 63 | src/estimator_example.cpp) 64 | add_dependencies(rosplane_estimator ${catkin_EXPORTED_TARGETS}) 65 | target_link_libraries(rosplane_estimator ${catkin_LIBRARIES}) 66 | 67 | 68 | ## Declare a C++ executable 69 | add_executable(rosplane_path_follower 70 | src/path_follower_example.cpp 71 | src/path_follower_base.cpp) 72 | add_dependencies(rosplane_path_follower ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) 73 | target_link_libraries(rosplane_path_follower ${catkin_LIBRARIES}) 74 | 75 | 76 | ## Declare a C++ executable 77 | add_executable(rosplane_path_manager 78 | src/path_manager_base.cpp 79 | src/path_manager_example.cpp) 80 | add_dependencies(rosplane_path_manager ${catkin_EXPORTED_TARGETS}) 81 | target_link_libraries(rosplane_path_manager ${catkin_LIBRARIES}) 82 | 83 | 84 | ## Declare a C++ executable 85 | add_executable(rosplane_path_planner 86 | src/path_planner.cpp) 87 | add_dependencies(rosplane_path_planner ${catkin_EXPORTED_TARGETS}) 88 | target_link_libraries(rosplane_path_planner ${catkin_LIBRARIES}) 89 | -------------------------------------------------------------------------------- /rosplane/cfg/Controller.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "rosplane" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # trim 9 | trim = gen.add_group("Trim") 10 | trim.add("TRIM_E", double_t, 0, "Elevator trim", 0, -1, 1) 11 | trim.add("TRIM_A", double_t, 0, "Aileron trim", 0, -1, 1) 12 | trim.add("TRIM_R", double_t, 0, "Rudder trim", 0, -1, 1) 13 | trim.add("TRIM_T", double_t, 0, "Throttle trim", 0.6, 0, 1) 14 | 15 | # course hold 16 | course = gen.add_group("Course") 17 | course.add("COURSE_KP", double_t, 0, "Course proportional gain", 0.7329, 0, 2) 18 | course.add("COURSE_KD", double_t, 0, "Course derivative gain", 0, -1, 0) 19 | course.add("COURSE_KI", double_t, 0, "Course integral gain", 0.0, 0, 0.2) 20 | 21 | # roll hold 22 | roll = gen.add_group("Roll") 23 | roll.add("ROLL_KP", double_t, 0, "Roll proportional gain", 1.17, 0, 3) 24 | roll.add("ROLL_KD", double_t, 0, "Roll derivative gain", -0.13, -1, 0) 25 | roll.add("ROLL_KI", double_t, 0, "Roll integral gain", 0, 0, 0.2) 26 | 27 | # pitch hold 28 | pitch = gen.add_group("Pitch") 29 | pitch.add("PITCH_KP", double_t, 0, "Pitch proportional gain", 1.0, 0, 3) 30 | pitch.add("PITCH_KD", double_t, 0, "Pitch derivative gain", -0.17, -0.4, 0) 31 | pitch.add("PITCH_KI", double_t, 0, "Pitch integral gain", 0, 0, 0.2) 32 | pitch.add("PITCH_FF", double_t, 0, "Pitch feed forward value", 0, -1, 1) 33 | 34 | # airspeed with pitch hold 35 | as_pitch = gen.add_group("Airspeed with Pitch") 36 | as_pitch.add("AS_PITCH_KP", double_t, 0, "Airspeed with pitch proportional gain", -0.0713, 0, 0.2) 37 | as_pitch.add("AS_PITCH_KD", double_t, 0, "Airspeed with pitch derivative gain", -0.0635, -0.2, 0) 38 | as_pitch.add("AS_PITCH_KI", double_t, 0, "Airspeed with pitch integral gain", 0, 0, 0.2) 39 | 40 | # airspeed with throttle hold 41 | as_thr = gen.add_group("Airspeed with Throttle") 42 | as_thr.add("AS_THR_KP", double_t, 0, "Airspeed with throttle proportional gain", 3.2, 0, 10) 43 | as_thr.add("AS_THR_KD", double_t, 0, "Airspeed with throttle derivative gain", 0, -5, 0) 44 | as_thr.add("AS_THR_KI", double_t, 0, "Airspeed with throttle integral gain", 1.0, 0, 10) 45 | 46 | # altitude hold 47 | alt = gen.add_group("Altitude") 48 | alt.add("ALT_KP", double_t, 0, "Altitude proportional gain", 0.045, 0, 0.1) 49 | alt.add("ALT_KD", double_t, 0, "Altitude derivative gain", 0, -0.05, 0) 50 | alt.add("ALT_KI", double_t, 0, "Altitude integral gain", 0.01, 0, 0.05) 51 | 52 | # side-slip hold 53 | sideslip = gen.add_group("Side Slip") 54 | sideslip.add("BETA_KP", double_t, 0, "Side slip proportional gain", -0.1164, 0, 0.3) 55 | sideslip.add("BETA_KD", double_t, 0, "Side slip derivative gain", 0, -0.15, 0) 56 | sideslip.add("BETA_KI", double_t, 0, "Side slip integral gain", -0.0037111, 0, 0.05) 57 | 58 | exit(gen.generate(PACKAGE, "rosplane", "Controller")) 59 | -------------------------------------------------------------------------------- /rosplane/cfg/Follower.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "rosplane" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # Chi Infinity 9 | gen.add("CHI_INFTY", double_t, 0, "Chi Infinity", 1.0472, 0 , 1.5708) 10 | 11 | # K Path 12 | gen.add("K_PATH", double_t, 0, "K Path", 0.025, 0, 1) 13 | 14 | # K Orbit 15 | gen.add("K_ORBIT", double_t, 0, "K Orbit", 4.0, 0, 15) 16 | 17 | exit(gen.generate(PACKAGE, "rosplane", "Follower")) 18 | -------------------------------------------------------------------------------- /rosplane/include/controller_base.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file controller_base.h 3 | * 4 | * Base class definition for autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php 5 | * 6 | * @author Gary Ellingson 7 | */ 8 | 9 | #ifndef CONTROLLER_BASE_H 10 | #define CONTROLLER_BASE_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | namespace rosplane 22 | { 23 | 24 | enum class alt_zones 25 | { 26 | TAKE_OFF, 27 | CLIMB, 28 | DESCEND, 29 | ALTITUDE_HOLD 30 | }; 31 | 32 | class controller_base 33 | { 34 | public: 35 | controller_base(); 36 | float spin(); 37 | 38 | protected: 39 | 40 | struct input_s 41 | { 42 | float Ts; /** time step */ 43 | float h; /** altitude */ 44 | float va; /** airspeed */ 45 | float phi; /** roll angle */ 46 | float theta; /** pitch angle */ 47 | float chi; /** course angle */ 48 | float p; /** body frame roll rate */ 49 | float q; /** body frame pitch rate */ 50 | float r; /** body frame yaw rate */ 51 | float Va_c; /** commanded airspeed (m/s) */ 52 | float h_c; /** commanded altitude (m) */ 53 | float chi_c; /** commanded course (rad) */ 54 | float phi_ff; /** feed forward term for orbits (rad) */ 55 | }; 56 | 57 | struct output_s 58 | { 59 | float theta_c; 60 | float delta_e; 61 | float phi_c; 62 | float delta_a; 63 | float delta_r; 64 | float delta_t; 65 | alt_zones current_zone; 66 | }; 67 | 68 | struct params_s 69 | { 70 | double alt_hz; /**< altitude hold zone */ 71 | double alt_toz; /**< altitude takeoff zone */ 72 | double tau; 73 | double c_kp; 74 | double c_kd; 75 | double c_ki; 76 | double r_kp; 77 | double r_kd; 78 | double r_ki; 79 | double p_kp; 80 | double p_kd; 81 | double p_ki; 82 | double p_ff; 83 | double a_p_kp; 84 | double a_p_kd; 85 | double a_p_ki; 86 | double a_t_kp; 87 | double a_t_kd; 88 | double a_t_ki; 89 | double a_kp; 90 | double a_kd; 91 | double a_ki; 92 | double b_kp; 93 | double b_kd; 94 | double b_ki; 95 | double trim_e; 96 | double trim_a; 97 | double trim_r; 98 | double trim_t; 99 | double max_e; 100 | double max_a; 101 | double max_r; 102 | double max_t; 103 | double pwm_rad_e; 104 | double pwm_rad_a; 105 | double pwm_rad_r; 106 | }; 107 | 108 | virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; 109 | 110 | private: 111 | ros::NodeHandle nh_; 112 | ros::NodeHandle nh_private_; 113 | ros::Subscriber vehicle_state_sub_; 114 | ros::Subscriber controller_commands_sub_; 115 | ros::Publisher actuators_pub_; 116 | ros::Publisher internals_pub_; 117 | ros::Timer act_pub_timer_; 118 | 119 | struct params_s params_; /**< params */ 120 | rosplane_msgs::Controller_Commands controller_commands_; 121 | rosplane_msgs::State vehicle_state_; 122 | 123 | void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg); 124 | void controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg); 125 | bool command_recieved_; 126 | 127 | dynamic_reconfigure::Server server_; 128 | dynamic_reconfigure::Server::CallbackType func_; 129 | 130 | void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level); 131 | 132 | /** 133 | * Convert from deflection angle to pwm 134 | */ 135 | void convert_to_pwm(struct output_s &output); 136 | 137 | /** 138 | * Publish the outputs 139 | */ 140 | void actuator_controls_publish(const ros::TimerEvent &); 141 | }; 142 | } //end namespace 143 | 144 | #endif // CONTROLLER_BASE_H 145 | -------------------------------------------------------------------------------- /rosplane/include/controller_example.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_EXAMPLE_H 2 | #define CONTROLLER_EXAMPLE_H 3 | 4 | #include "controller_base.h" 5 | 6 | namespace rosplane 7 | { 8 | 9 | class controller_example : public controller_base 10 | { 11 | public: 12 | controller_example(); 13 | private: 14 | virtual void control(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); 15 | alt_zones current_zone; 16 | 17 | float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s ¶ms, float Ts); 18 | float c_error_; 19 | float c_integrator_; 20 | 21 | float roll_hold(float phi_c, float phi, float p, const struct params_s ¶ms, float Ts); 22 | float r_error_; 23 | float r_integrator; 24 | 25 | float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms, float Ts); 26 | float p_error_; 27 | float p_integrator_; 28 | 29 | float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts); 30 | float ap_error_; 31 | float ap_integrator_; 32 | float ap_differentiator_; 33 | 34 | float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts); 35 | float at_error_; 36 | float at_integrator_; 37 | float at_differentiator_; 38 | 39 | float altitiude_hold(float h_c, float h, const struct params_s ¶ms, float Ts); 40 | float a_error_; 41 | float a_integrator_; 42 | float a_differentiator_; 43 | 44 | // float cooridinated_turn_hold(float v, const struct params_s ¶ms, float Ts); 45 | // float ct_error_; 46 | // float ct_integrator_; 47 | // float ct_differentiator_; 48 | 49 | float sat(float value, float up_limit, float low_limit); 50 | }; 51 | } //end namespace 52 | 53 | #endif // CONTROLLER_EXAMPLE_H 54 | -------------------------------------------------------------------------------- /rosplane/include/estimator_base.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file estimator_base.h 3 | * 4 | * Base class definition for autopilot estimator in chapter 8 of UAVbook, see http://uavbook.byu.edu/doku.php 5 | * 6 | * @author Gary Ellingson 7 | */ 8 | 9 | #ifndef ESTIMATOR_BASE_H 10 | #define ESTIMATOR_BASE_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #define EARTH_RADIUS 6378145.0f 25 | 26 | namespace rosplane 27 | { 28 | 29 | 30 | class estimator_base 31 | { 32 | public: 33 | estimator_base(); 34 | 35 | protected: 36 | 37 | struct input_s 38 | { 39 | float gyro_x; 40 | float gyro_y; 41 | float gyro_z; 42 | float accel_x; 43 | float accel_y; 44 | float accel_z; 45 | float static_pres; 46 | float diff_pres; 47 | bool gps_new; 48 | float gps_n; 49 | float gps_e; 50 | float gps_h; 51 | float gps_Vg; 52 | float gps_course; 53 | bool status_armed; 54 | bool armed_init; 55 | }; 56 | 57 | struct output_s 58 | { 59 | float pn; 60 | float pe; 61 | float h; 62 | float Va; 63 | float alpha; 64 | float beta; 65 | float phi; 66 | float theta; 67 | float psi; 68 | float chi; 69 | float p; 70 | float q; 71 | float r; 72 | float Vg; 73 | float wn; 74 | float we; 75 | }; 76 | 77 | struct params_s 78 | { 79 | double gravity; 80 | double rho; 81 | double sigma_accel; 82 | double sigma_n_gps; 83 | double sigma_e_gps; 84 | double sigma_Vg_gps; 85 | double sigma_course_gps; 86 | double Ts; 87 | }; 88 | 89 | virtual void estimate(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; 90 | 91 | private: 92 | ros::NodeHandle nh_; 93 | ros::NodeHandle nh_private_; 94 | ros::Publisher vehicle_state_pub_; 95 | ros::Subscriber gnss_fix_sub_; 96 | ros::Subscriber gnss_vel_sub_; //used in conjunction with the gnss_fix_sub_ 97 | ros::Subscriber imu_sub_; 98 | ros::Subscriber baro_sub_; 99 | ros::Subscriber airspeed_sub_; 100 | ros::Subscriber status_sub_; 101 | 102 | void update(const ros::TimerEvent &); 103 | void gnssFixCallback(const sensor_msgs::NavSatFix &msg); 104 | void gnssVelCallback(const geometry_msgs::TwistStamped &msg); 105 | void imuCallback(const sensor_msgs::Imu &msg); 106 | void baroAltCallback(const rosflight_msgs::Barometer &msg); 107 | void airspeedCallback(const rosflight_msgs::Airspeed &msg); 108 | void statusCallback(const rosflight_msgs::Status &msg); 109 | 110 | double update_rate_; 111 | ros::Timer update_timer_; 112 | std::string gnss_fix_topic_; 113 | std::string gnss_vel_topic_; 114 | std::string imu_topic_; 115 | std::string baro_topic_; 116 | std::string airspeed_topic_; 117 | std::string status_topic_; 118 | 119 | bool gps_new_; 120 | bool gps_init_; 121 | double init_lat_; /**< Initial latitude in degrees */ 122 | double init_lon_; /**< Initial longitude in degrees */ 123 | float init_alt_; /**< Initial altitude in meters above MSL */ 124 | bool armed_first_time_; /**< Arm before starting estimation */ 125 | bool baro_init_; /**< Initial barometric pressure */ 126 | float init_static_; /**< Initial static pressure (mbar) */ 127 | int baro_count_; /**< Used to grab the first set of baro measurements */ 128 | std::vector init_static_vector_; /**< Used to grab the first set of baro measurements */ 129 | 130 | struct params_s params_; 131 | struct input_s input_; 132 | }; 133 | 134 | } //end namespace 135 | 136 | #endif // ESTIMATOR_BASE_H 137 | -------------------------------------------------------------------------------- /rosplane/include/estimator_example.h: -------------------------------------------------------------------------------- 1 | #ifndef ESTIMATOR_EXAMPLE_H 2 | #define ESTIMATOR_EXAMPLE_H 3 | 4 | #include "estimator_base.h" 5 | 6 | #include 7 | #include 8 | 9 | namespace rosplane 10 | { 11 | 12 | class estimator_example : public estimator_base 13 | { 14 | public: 15 | estimator_example(); 16 | 17 | private: 18 | virtual void estimate(const params_s ¶ms, const input_s &input, output_s &output); 19 | 20 | // float gps_n_old_; 21 | // float gps_e_old_; 22 | // float gps_Vg_old_; 23 | // float gps_course_old_; 24 | 25 | double lpf_a_; 26 | float alpha_; 27 | float alpha1_; 28 | int N_; 29 | 30 | float lpf_gyro_x_; 31 | float lpf_gyro_y_; 32 | float lpf_gyro_z_; 33 | float lpf_static_; 34 | float lpf_diff_; 35 | float lpf_accel_x_; 36 | float lpf_accel_y_; 37 | float lpf_accel_z_; 38 | 39 | float phat_; 40 | float qhat_; 41 | float rhat_; 42 | float Vwhat_; 43 | float phihat_; 44 | float thetahat_; 45 | float psihat_; 46 | Eigen::Vector2f xhat_a_; // 2 47 | Eigen::Matrix2f P_a_; // 2x2 48 | 49 | Eigen::VectorXf xhat_p_; // 7 50 | Eigen::MatrixXf P_p_; // 7x7 51 | 52 | Eigen::Matrix2f Q_a_; // 2x2 53 | float R_accel_; 54 | Eigen::Vector2f f_a_; // 2 55 | Eigen::Matrix2f A_a_; // 2x2 56 | float h_a_; 57 | Eigen::Vector2f C_a_; // 2 58 | Eigen::Vector2f L_a_; // 2 59 | 60 | Eigen::MatrixXf Q_p_; // 7x7 61 | Eigen::MatrixXf R_p_; // 6x6 62 | Eigen::VectorXf f_p_; // 7 63 | Eigen::MatrixXf A_p_; // 7x7 64 | float h_p_; 65 | Eigen::VectorXf C_p_; // 7 66 | Eigen::VectorXf L_p_; // 7 67 | 68 | void check_xhat_a(); 69 | 70 | }; 71 | } //end namespace 72 | 73 | #endif // ESTIMATOR_EXAMPLE_H 74 | -------------------------------------------------------------------------------- /rosplane/include/path_follower_base.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_FOLLOWER_BASE_H 2 | #define PATH_FOLLOWER_BASE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | 12 | namespace rosplane 13 | { 14 | 15 | enum class path_type 16 | { 17 | Orbit, 18 | Line 19 | }; 20 | 21 | class path_follower_base 22 | { 23 | public: 24 | path_follower_base(); 25 | float spin(); 26 | 27 | protected: 28 | 29 | struct input_s 30 | { 31 | enum path_type p_type; 32 | float Va_d; 33 | float r_path[3]; 34 | float q_path[3]; 35 | float c_orbit[3]; 36 | float rho_orbit; 37 | int lam_orbit; 38 | float pn; /** position north */ 39 | float pe; /** position east */ 40 | float h; /** altitude */ 41 | float Va; /** airspeed */ 42 | float chi; /** course angle */ 43 | }; 44 | 45 | struct output_s 46 | { 47 | double Va_c; /** commanded airspeed (m/s) */ 48 | double h_c; /** commanded altitude (m) */ 49 | double chi_c; /** commanded course (rad) */ 50 | double phi_ff; /** feed forward term for orbits (rad) */ 51 | }; 52 | 53 | struct params_s 54 | { 55 | double chi_infty; 56 | double k_path; 57 | double k_orbit; 58 | }; 59 | 60 | virtual void follow(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; 61 | 62 | private: 63 | ros::NodeHandle nh_; 64 | ros::NodeHandle nh_private_; 65 | ros::Subscriber vehicle_state_sub_; 66 | ros::Subscriber current_path_sub_; 67 | 68 | ros::Publisher controller_commands_pub_; 69 | 70 | double update_rate_ = 100.0; 71 | ros::Timer update_timer_; 72 | 73 | rosplane_msgs::Controller_Commands controller_commands_; 74 | struct params_s params_; /**< params */ 75 | struct input_s input_; 76 | 77 | void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg); 78 | bool state_init_; 79 | void current_path_callback(const rosplane_msgs::Current_PathConstPtr &msg); 80 | bool current_path_init_; 81 | 82 | dynamic_reconfigure::Server server_; 83 | dynamic_reconfigure::Server::CallbackType func_; 84 | void reconfigure_callback(rosplane::FollowerConfig &config, uint32_t level); 85 | 86 | void update(const ros::TimerEvent &); 87 | }; 88 | 89 | } // end namespace 90 | 91 | #endif // PATH_FOLLOWER_BASE_H 92 | -------------------------------------------------------------------------------- /rosplane/include/path_follower_example.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_FOLLOWER_EXAMPLE_H 2 | #define PATH_FOLLOWER_EXAMPLE_H 3 | 4 | #include "path_follower_base.h" 5 | 6 | namespace rosplane 7 | { 8 | 9 | class path_follower_example : public path_follower_base 10 | { 11 | public: 12 | path_follower_example(); 13 | private: 14 | virtual void follow(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); 15 | }; 16 | 17 | } //end namespace 18 | #endif // PATH_FOLLOWER_EXAMPLE_H 19 | -------------------------------------------------------------------------------- /rosplane/include/path_manager_base.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file path_manager_base.h 3 | * 4 | * Base class definition for autopilot path follower in chapter 10 of UAVbook, see http://uavbook.byu.edu/doku.php 5 | * 6 | * @author Gary Ellingson 7 | * adapted by Judd Mehr and Brian Russel for RosPlane software 8 | */ 9 | 10 | #ifndef PATH_MANAGER_BASE_H 11 | #define PATH_MANAGER_BASE_H 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace rosplane 26 | { 27 | class path_manager_base 28 | { 29 | public: 30 | path_manager_base(); 31 | 32 | protected: 33 | 34 | struct waypoint_s 35 | { 36 | float w[3]; 37 | float chi_d; 38 | bool chi_valid; 39 | float Va_d; 40 | }; 41 | 42 | std::vector waypoints_; 43 | int num_waypoints_; 44 | int idx_a_; /** index to the waypoint that was most recently achieved */ 45 | 46 | struct input_s 47 | { 48 | float pn; /** position north */ 49 | float pe; /** position east */ 50 | float h; /** altitude */ 51 | float chi; /** course angle */ 52 | }; 53 | 54 | struct output_s 55 | { 56 | bool flag; /** Inicates strait line or orbital path (true is line, false is orbit) */ 57 | float Va_d; /** Desired airspeed (m/s) */ 58 | float r[3]; /** Vector to origin of straight line path (m) */ 59 | float q[3]; /** Unit vector, desired direction of travel for line path */ 60 | float c[3]; /** Center of orbital path (m) */ 61 | float rho; /** Radius of orbital path (m) */ 62 | int8_t lambda; /** Direction of orbital path (cw is 1, ccw is -1) */ 63 | }; 64 | 65 | struct params_s 66 | { 67 | double R_min; 68 | }; 69 | 70 | virtual void manage(const struct params_s ¶ms, const struct input_s &input, struct output_s &output) = 0; 71 | 72 | private: 73 | 74 | ros::NodeHandle nh_; 75 | ros::NodeHandle nh_private_; 76 | ros::Subscriber vehicle_state_sub_; /**< vehicle state subscription */ 77 | ros::Subscriber new_waypoint_sub_; /**< new waypoint subscription */ 78 | ros::Publisher current_path_pub_; /**< controller commands publication */ 79 | 80 | struct params_s params_; 81 | 82 | rosplane_msgs::State vehicle_state_; /**< vehicle state */ 83 | 84 | double update_rate_; 85 | ros::Timer update_timer_; 86 | 87 | void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg); 88 | bool state_init_; 89 | void new_waypoint_callback(const rosplane_msgs::Waypoint &msg); 90 | void current_path_publish(const ros::TimerEvent &); 91 | }; 92 | } //end namespace 93 | #endif // PATH_MANAGER_BASE_H 94 | -------------------------------------------------------------------------------- /rosplane/include/path_manager_example.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_MANAGER_EXAMPLE_H 2 | #define PATH_MANAGER_EXAMPLE_H 3 | 4 | #include "path_manager_base.h" 5 | #include 6 | //#include 7 | 8 | 9 | #define M_PI_F 3.14159265358979323846f 10 | #define M_PI_2_F 1.57079632679489661923f 11 | namespace rosplane 12 | { 13 | enum class fillet_state 14 | { 15 | STRAIGHT, 16 | ORBIT 17 | }; 18 | 19 | enum class dubin_state 20 | { 21 | FIRST, 22 | BEFORE_H1, 23 | BEFORE_H1_WRONG_SIDE, 24 | STRAIGHT, 25 | BEFORE_H3, 26 | BEFORE_H3_WRONG_SIDE 27 | }; 28 | 29 | class path_manager_example : public path_manager_base 30 | { 31 | public: 32 | path_manager_example(); 33 | private: 34 | virtual void manage(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); 35 | 36 | void manage_line(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); 37 | void manage_fillet(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); 38 | fillet_state fil_state_; 39 | void manage_dubins(const struct params_s ¶ms, const struct input_s &input, struct output_s &output); 40 | dubin_state dub_state_; 41 | struct dubinspath_s 42 | { 43 | 44 | Eigen::Vector3f ps; /** the start position */ 45 | float chis; /** the start course angle */ 46 | Eigen::Vector3f pe; /** the end position */ 47 | float chie; /** the end course angle */ 48 | float R; /** turn radius */ 49 | float L; /** length of the path */ 50 | Eigen::Vector3f cs; /** center of the start circle */ 51 | int lams; /** direction of the start circle */ 52 | Eigen::Vector3f ce; /** center of the endcircle */ 53 | int lame; /** direction of the end circle */ 54 | Eigen::Vector3f w1; /** vector defining half plane H1 */ 55 | Eigen::Vector3f q1; /** unit vector along striaght line path */ 56 | Eigen::Vector3f w2; /** vector defining half plane H2 */ 57 | Eigen::Vector3f w3; /** vector defining half plane H3 */ 58 | Eigen::Vector3f q3; /** unit vector defining direction of half plane H3 */ 59 | }; 60 | struct dubinspath_s dubinspath_; 61 | void dubinsParameters(const struct waypoint_s start_node, const struct waypoint_s end_node, float R); 62 | 63 | Eigen::Matrix3f rotz(float theta); 64 | float mo(float in); 65 | }; 66 | } //end namespace 67 | #endif // PATH_MANAGER_EXAMPLE_H 68 | -------------------------------------------------------------------------------- /rosplane/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosplane 4 | 0.0.0 5 | The ROSplane package 6 | 7 | Gary Ellingson 8 | 9 | Gary Ellingson 10 | 11 | BSD 12 | 13 | catkin 14 | cmake_modules 15 | dynamic_reconfigure 16 | rosflight_msgs 17 | rosplane_msgs 18 | roscpp 19 | rospy 20 | sensor_msgs 21 | geometry_msgs 22 | dynamic_reconfigure 23 | rosflight_msgs 24 | rosplane_msgs 25 | roscpp 26 | rospy 27 | sensor_msgs 28 | geometry_msgs 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /rosplane/param/aerosonde.yaml: -------------------------------------------------------------------------------- 1 | TRIM_E: 0 2 | TRIM_A: 0 3 | TRIM_R: 0 4 | TRIM_T: 0.6 5 | 6 | COURSE_KP: 0.7329 7 | COURSE_KD: 0 8 | COURSE_KI: 0.07 9 | 10 | ROLL_KP: 1.17 11 | ROLL_KD: -0.13 12 | ROLL_KI: 0 13 | 14 | PITCH_KP: 0.1 15 | PITCH_KD: -0.017 16 | PITCH_KI: 0 17 | PITCH_FF: 0 18 | 19 | AS_PITCH_KP: -0.0713 20 | AS_PITCH_KD: -0.0635 21 | AS_PITCH_KI: 0 22 | 23 | AS_THR_KP: 3.2 24 | AS_THR_KD: 0 25 | AS_THR_KI: 1.0 26 | 27 | ALT_KP: 0.045 28 | ALT_KD: 0 29 | ALT_KI: 0.01 30 | 31 | BETA_KP: -0.1164 32 | BETA_KD: 0 33 | BETA_KI: -0.0037111 34 | -------------------------------------------------------------------------------- /rosplane/param/bixler.yaml: -------------------------------------------------------------------------------- 1 | TRIM_E: 0 2 | TRIM_A: 0 3 | TRIM_R: 0 4 | TRIM_T: 0.6 5 | 6 | COURSE_KP: 0.7329 7 | COURSE_KD: 0 8 | COURSE_KI: 0.0 9 | 10 | ROLL_KP: 1.2855 11 | ROLL_KD: -0.1 12 | ROLL_KI: 0 13 | 14 | PITCH_KP: 1.0 15 | PITCH_KD: -0.17 16 | PITCH_KI: 0 17 | PITCH_FF: 0 18 | 19 | AS_PITCH_KP: -0.0713 20 | AS_PITCH_KD: -0.0635 21 | AS_PITCH_KI: 0 22 | 23 | 24 | AS_THR_KP: 3.2 25 | AS_THR_KD: 0 26 | AS_THR_KI: 1.0 27 | 28 | 29 | ALT_KP: 0.045 30 | ALT_KD: 0 31 | ALT_KI: 0.01 32 | 33 | 34 | BETA_KP: -0.1164 35 | BETA_KD: 0 36 | BETA_KI: -0.0037111 37 | 38 | 39 | CHI_INFTY: 1.0472 40 | K_PATH: 0.025 41 | K_ORBIT: 2.00 42 | -------------------------------------------------------------------------------- /rosplane/src/controller_base.cpp: -------------------------------------------------------------------------------- 1 | #include "controller_base.h" 2 | #include "controller_example.h" 3 | 4 | namespace rosplane 5 | { 6 | 7 | controller_base::controller_base(): 8 | nh_(), 9 | nh_private_("~") 10 | { 11 | vehicle_state_sub_ = nh_.subscribe("state", 10, &controller_base::vehicle_state_callback, this); 12 | controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &controller_base::controller_commands_callback, 13 | this); 14 | 15 | memset(&vehicle_state_, 0, sizeof(vehicle_state_)); 16 | memset(&controller_commands_, 0, sizeof(controller_commands_)); 17 | 18 | nh_private_.param("TRIM_E", params_.trim_e, 0.0); 19 | nh_private_.param("TRIM_A", params_.trim_a, 0.0); 20 | nh_private_.param("TRIM_R", params_.trim_r, 0.0); 21 | nh_private_.param("TRIM_T", params_.trim_t, 0.6); 22 | nh_private_.param("PWM_RAD_E", params_.pwm_rad_e, 1.0); 23 | nh_private_.param("PWM_RAD_A", params_.pwm_rad_a, 1.0); 24 | nh_private_.param("PWM_RAD_R", params_.pwm_rad_r, 1.0); 25 | nh_private_.param("ALT_TOZ", params_.alt_toz, 20.0); 26 | nh_private_.param("ALT_HZ", params_.alt_hz, 10.0); 27 | nh_private_.param("TAU", params_.tau, 5.0); 28 | nh_private_.param("COURSE_KP", params_.c_kp, 0.7329); 29 | nh_private_.param("COURSE_KD", params_.c_kd, 0.0); 30 | nh_private_.param("COURSE_KI", params_.c_ki, 0.0); 31 | nh_private_.param("ROLL_KP", params_.r_kp, 1.2855); 32 | nh_private_.param("ROLL_KD", params_.r_kd, -0.325); 33 | nh_private_.param("ROLL_KI", params_.r_ki, 0.0);//0.10f); 34 | nh_private_.param("PITCH_KP", params_.p_kp, 1.0); 35 | nh_private_.param("PITCH_KD", params_.p_kd, -0.17); 36 | nh_private_.param("PITCH_KI", params_.p_ki, 0.0); 37 | nh_private_.param("PITCH_FF", params_.p_ff, 0.0); 38 | nh_private_.param("AS_PITCH_KP", params_.a_p_kp, -0.0713); 39 | nh_private_.param("AS_PITCH_KD", params_.a_p_kd, -0.0635); 40 | nh_private_.param("AS_PITCH_KI", params_.a_p_ki, 0.0); 41 | nh_private_.param("AS_THR_KP", params_.a_t_kp, 3.2); 42 | nh_private_.param("AS_THR_KD", params_.a_t_kd, 0.0); 43 | nh_private_.param("AS_THR_KI", params_.a_t_ki, 0.0); 44 | nh_private_.param("ALT_KP", params_.a_kp, 0.045); 45 | nh_private_.param("ALT_KD", params_.a_kd, 0.0); 46 | nh_private_.param("ALT_KI", params_.a_ki, 0.01); 47 | nh_private_.param("BETA_KP", params_.b_kp, -0.1164); 48 | nh_private_.param("BETA_KD", params_.b_kd, 0.0); 49 | nh_private_.param("BETA_KI", params_.b_ki, -0.0037111); 50 | nh_private_.param("MAX_E", params_.max_e, 0.610); 51 | nh_private_.param("MAX_A", params_.max_a, 0.523); 52 | nh_private_.param("MAX_R", params_.max_r, 0.523); 53 | nh_private_.param("MAX_T", params_.max_t, 1.0); 54 | 55 | func_ = boost::bind(&controller_base::reconfigure_callback, this, _1, _2); 56 | server_.setCallback(func_); 57 | 58 | actuators_pub_ = nh_.advertise("command", 10); 59 | internals_pub_ = nh_.advertise("controller_inners", 10); 60 | act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base::actuator_controls_publish, this); 61 | 62 | command_recieved_ = false; 63 | } 64 | 65 | void controller_base::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg) 66 | { 67 | vehicle_state_ = *msg; 68 | } 69 | 70 | void controller_base::controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg) 71 | { 72 | command_recieved_ = true; 73 | controller_commands_ = *msg; 74 | } 75 | 76 | void controller_base::reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level) 77 | { 78 | params_.trim_e = config.TRIM_E; 79 | params_.trim_a = config.TRIM_A; 80 | params_.trim_r = config.TRIM_R; 81 | params_.trim_t = config.TRIM_T; 82 | 83 | params_.c_kp = config.COURSE_KP; 84 | params_.c_kd = config.COURSE_KD; 85 | params_.c_ki = config.COURSE_KI; 86 | 87 | params_.r_kp = config.ROLL_KP; 88 | params_.r_kd = config.ROLL_KD; 89 | params_.r_ki = config.ROLL_KI; 90 | 91 | params_.p_kp = config.PITCH_KP; 92 | params_.p_kd = config.PITCH_KD; 93 | params_.p_ki = config.PITCH_KI; 94 | params_.p_ff = config.PITCH_FF; 95 | 96 | params_.a_p_kp = config.AS_PITCH_KP; 97 | params_.a_p_kd = config.AS_PITCH_KD; 98 | params_.a_p_ki = config.AS_PITCH_KI; 99 | 100 | params_.a_t_kp = config.AS_THR_KP; 101 | params_.a_t_kd = config.AS_THR_KD; 102 | params_.a_t_ki = config.AS_THR_KI; 103 | 104 | params_.a_kp = config.ALT_KP; 105 | params_.a_kd = config.ALT_KD; 106 | params_.a_ki = config.ALT_KI; 107 | 108 | params_.b_kp = config.BETA_KP; 109 | params_.b_kd = config.BETA_KD; 110 | params_.b_ki = config.BETA_KI; 111 | } 112 | 113 | void controller_base::convert_to_pwm(controller_base::output_s &output) 114 | { 115 | output.delta_e = output.delta_e*params_.pwm_rad_e; 116 | output.delta_a = output.delta_a*params_.pwm_rad_a; 117 | output.delta_r = output.delta_r*params_.pwm_rad_r; 118 | } 119 | 120 | void controller_base::actuator_controls_publish(const ros::TimerEvent &) 121 | { 122 | struct input_s input; 123 | input.h = -vehicle_state_.position[2]; 124 | input.va = vehicle_state_.Va; 125 | input.phi = vehicle_state_.phi; 126 | input.theta = vehicle_state_.theta; 127 | input.chi = vehicle_state_.chi; 128 | input.p = vehicle_state_.p; 129 | input.q = vehicle_state_.q; 130 | input.r = vehicle_state_.r; 131 | input.Va_c = controller_commands_.Va_c; 132 | input.h_c = controller_commands_.h_c; 133 | input.chi_c = controller_commands_.chi_c; 134 | input.phi_ff = controller_commands_.phi_ff; 135 | input.Ts = 0.01f; 136 | 137 | struct output_s output; 138 | if (command_recieved_ == true) 139 | { 140 | control(params_, input, output); 141 | 142 | convert_to_pwm(output); 143 | 144 | rosflight_msgs::Command actuators; 145 | /* publish actuator controls */ 146 | 147 | actuators.ignore = 0; 148 | actuators.mode = rosflight_msgs::Command::MODE_PASS_THROUGH; 149 | actuators.x = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f; 150 | actuators.y = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f; 151 | actuators.z = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f; 152 | actuators.F = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f; 153 | 154 | actuators_pub_.publish(actuators); 155 | 156 | if (internals_pub_.getNumSubscribers() > 0) 157 | { 158 | rosplane_msgs::Controller_Internals inners; 159 | inners.phi_c = output.phi_c; 160 | inners.theta_c = output.theta_c; 161 | switch (output.current_zone) 162 | { 163 | case alt_zones::TAKE_OFF: 164 | inners.alt_zone = inners.ZONE_TAKE_OFF; 165 | break; 166 | case alt_zones::CLIMB: 167 | inners.alt_zone = inners.ZONE_CLIMB; 168 | break; 169 | case alt_zones::DESCEND: 170 | inners.alt_zone = inners.ZONE_DESEND; 171 | break; 172 | case alt_zones::ALTITUDE_HOLD: 173 | inners.alt_zone = inners.ZONE_ALTITUDE_HOLD; 174 | break; 175 | default: 176 | break; 177 | } 178 | inners.aux_valid = false; 179 | internals_pub_.publish(inners); 180 | } 181 | } 182 | } 183 | 184 | } //end namespace 185 | 186 | int main(int argc, char **argv) 187 | { 188 | ros::init(argc, argv, "rosplane_controller"); 189 | rosplane::controller_base *cont = new rosplane::controller_example(); 190 | 191 | ros::spin(); 192 | 193 | return 0; 194 | } 195 | -------------------------------------------------------------------------------- /rosplane/src/controller_example.cpp: -------------------------------------------------------------------------------- 1 | #include "controller_example.h" 2 | 3 | namespace rosplane 4 | { 5 | 6 | controller_example::controller_example() : controller_base() 7 | { 8 | current_zone = alt_zones::TAKE_OFF; 9 | 10 | c_error_ = 0; 11 | c_integrator_ = 0; 12 | r_error_ = 0; 13 | r_integrator = 0; 14 | p_error_ = 0; 15 | p_integrator_ = 0; 16 | 17 | } 18 | 19 | void controller_example::control(const params_s ¶ms, const input_s &input, output_s &output) 20 | { 21 | output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts) 22 | output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts); 23 | output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts); 24 | 25 | switch (current_zone) 26 | { 27 | case alt_zones::TAKE_OFF: 28 | output.phi_c = 0; 29 | output.delta_a = roll_hold(0.0, input.phi, input.p, params, input.Ts); 30 | output.delta_t = params.max_t; 31 | output.theta_c = 15.0*3.14/180.0; 32 | if (input.h >= params.alt_toz) 33 | { 34 | ROS_DEBUG("climb"); 35 | current_zone = alt_zones::CLIMB; 36 | ap_error_ = 0; 37 | ap_integrator_ = 0; 38 | ap_differentiator_ = 0; 39 | } 40 | break; 41 | case alt_zones::CLIMB: 42 | output.delta_t = params.max_t; 43 | output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts); 44 | if (input.h >= input.h_c - params.alt_hz) 45 | { 46 | ROS_DEBUG("hold"); 47 | current_zone = alt_zones::ALTITUDE_HOLD; 48 | at_error_ = 0; 49 | at_integrator_ = 0; 50 | at_differentiator_ = 0; 51 | a_error_ = 0; 52 | a_integrator_ = 0; 53 | a_differentiator_ = 0; 54 | } 55 | else if (input.h <= params.alt_toz) 56 | { 57 | ROS_DEBUG("takeoff"); 58 | current_zone = alt_zones::TAKE_OFF; 59 | } 60 | break; 61 | case alt_zones::DESCEND: 62 | output.delta_t = 0; 63 | output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts); 64 | if (input.h <= input.h_c + params.alt_hz) 65 | { 66 | ROS_DEBUG("hold"); 67 | current_zone = alt_zones::ALTITUDE_HOLD; 68 | at_error_ = 0; 69 | at_integrator_ = 0; 70 | at_differentiator_ = 0; 71 | a_error_ = 0; 72 | a_integrator_ = 0; 73 | a_differentiator_ = 0; 74 | } 75 | break; 76 | case alt_zones::ALTITUDE_HOLD: 77 | output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts); 78 | output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts); 79 | if (input.h >= input.h_c + params.alt_hz) 80 | { 81 | ROS_DEBUG("desend"); 82 | current_zone = alt_zones::DESCEND; 83 | ap_error_ = 0; 84 | ap_integrator_ = 0; 85 | ap_differentiator_ = 0; 86 | } 87 | else if (input.h <= input.h_c - params.alt_hz) 88 | { 89 | ROS_DEBUG("climb"); 90 | current_zone = alt_zones::CLIMB; 91 | ap_error_ = 0; 92 | ap_integrator_ = 0; 93 | ap_differentiator_ = 0; 94 | } 95 | break; 96 | default: 97 | break; 98 | } 99 | 100 | output.current_zone = current_zone; 101 | output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts); 102 | } 103 | 104 | float controller_example::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s ¶ms, float Ts) 105 | { 106 | float error = chi_c - chi; 107 | 108 | c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_); 109 | 110 | float up = params.c_kp*error; 111 | float ui = params.c_ki*c_integrator_; 112 | float ud = params.c_kd*r; 113 | 114 | float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0); 115 | if (fabs(params.c_ki) >= 0.00001) 116 | { 117 | float phi_c_unsat = up + ui + ud + phi_ff; 118 | c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat); 119 | } 120 | 121 | c_error_ = error; 122 | return phi_c; 123 | } 124 | 125 | float controller_example::roll_hold(float phi_c, float phi, float p, const params_s ¶ms, float Ts) 126 | { 127 | float error = phi_c - phi; 128 | 129 | r_integrator = r_integrator + (Ts/2.0)*(error + r_error_); 130 | 131 | float up = params.r_kp*error; 132 | float ui = params.r_ki*r_integrator; 133 | float ud = params.r_kd*p; 134 | 135 | float delta_a = sat(up + ui + ud, params.max_a, -params.max_a); 136 | if (fabs(params.r_ki) >= 0.00001) 137 | { 138 | float delta_a_unsat = up + ui + ud; 139 | r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat); 140 | } 141 | 142 | r_error_ = error; 143 | return delta_a; 144 | } 145 | 146 | float controller_example::pitch_hold(float theta_c, float theta, float q, const params_s ¶ms, float Ts) 147 | { 148 | float error = theta_c - theta; 149 | 150 | p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_); 151 | 152 | float up = params.p_kp*error; 153 | float ui = params.p_ki*p_integrator_; 154 | float ud = params.p_kd*q; 155 | 156 | float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, params.max_e, -params.max_e); 157 | if (fabs(params.p_ki) >= 0.00001) 158 | { 159 | float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui + ud; 160 | p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat); 161 | } 162 | 163 | p_error_ = error; 164 | return delta_e; 165 | } 166 | 167 | float controller_example::airspeed_with_pitch_hold(float Va_c, float Va, const params_s ¶ms, float Ts) 168 | { 169 | float error = Va_c - Va; 170 | 171 | ap_integrator_ = ap_integrator_ + (Ts/2.0)*(error + ap_error_); 172 | ap_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*ap_differentiator_ + (2.0 / 173 | (2.0*params.tau + Ts))*(error - ap_error_); 174 | 175 | float up = params.a_p_kp*error; 176 | float ui = params.a_p_ki*ap_integrator_; 177 | float ud = params.a_p_kd*ap_differentiator_; 178 | 179 | float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -25.0*3.14/180.0); 180 | if (fabs(params.a_p_ki) >= 0.00001) 181 | { 182 | float theta_c_unsat = up + ui + ud; 183 | ap_integrator_ = ap_integrator_ + (Ts/params.a_p_ki)*(theta_c - theta_c_unsat); 184 | } 185 | 186 | ap_error_ = error; 187 | return theta_c; 188 | } 189 | 190 | float controller_example::airspeed_with_throttle_hold(float Va_c, float Va, const params_s ¶ms, float Ts) 191 | { 192 | float error = Va_c - Va; 193 | 194 | at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_); 195 | at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 / 196 | (2.0*params.tau + Ts))*(error - at_error_); 197 | 198 | float up = params.a_t_kp*error; 199 | float ui = params.a_t_ki*at_integrator_; 200 | float ud = params.a_t_kd*at_differentiator_; 201 | 202 | float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0); 203 | if (fabs(params.a_t_ki) >= 0.00001) 204 | { 205 | float delta_t_unsat = params.trim_t + up + ui + ud; 206 | at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat); 207 | } 208 | 209 | at_error_ = error; 210 | return delta_t; 211 | } 212 | 213 | float controller_example::altitiude_hold(float h_c, float h, const params_s ¶ms, float Ts) 214 | { 215 | float error = h_c - h; 216 | 217 | a_integrator_ = a_integrator_ + (Ts/2.0)*(error + a_error_); 218 | a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 / 219 | (2.0*params.tau + Ts))*(error - a_error_); 220 | 221 | float up = params.a_kp*error; 222 | float ui = params.a_ki*a_integrator_; 223 | float ud = params.a_kd*a_differentiator_; 224 | 225 | float theta_c = sat(up + ui + ud, 35.0*3.14/180.0, -35.0*3.14/180.0); 226 | if (fabs(params.a_ki) >= 0.00001) 227 | { 228 | float theta_c_unsat = up + ui + ud; 229 | a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat); 230 | } 231 | 232 | at_error_ = error; 233 | return theta_c; 234 | } 235 | 236 | //float controller_example::cooridinated_turn_hold(float v, const params_s ¶ms, float Ts) 237 | //{ 238 | // //todo finish this if you want... 239 | // return 0; 240 | //} 241 | 242 | float controller_example::sat(float value, float up_limit, float low_limit) 243 | { 244 | float rVal; 245 | if (value > up_limit) 246 | rVal = up_limit; 247 | else if (value < low_limit) 248 | rVal = low_limit; 249 | else 250 | rVal = value; 251 | 252 | return rVal; 253 | } 254 | 255 | } //end namespace 256 | -------------------------------------------------------------------------------- /rosplane/src/estimator_base.cpp: -------------------------------------------------------------------------------- 1 | #include "estimator_base.h" 2 | #include "estimator_example.h" 3 | #include 4 | 5 | namespace rosplane 6 | { 7 | 8 | estimator_base::estimator_base(): 9 | nh_(ros::NodeHandle()), 10 | nh_private_(ros::NodeHandle("~")) 11 | { 12 | nh_private_.param("gps_topic", gnss_fix_topic_, "navsat_compat/fix"); 13 | nh_private_.param("vel_topic", gnss_vel_topic_, "navsat_compat/vel"); 14 | nh_private_.param("imu_topic", imu_topic_, "imu/data"); 15 | nh_private_.param("baro_topic", baro_topic_, "baro"); 16 | nh_private_.param("airspeed_topic", airspeed_topic_, "airspeed"); 17 | nh_private_.param("status_topic", status_topic_, "status"); 18 | nh_private_.param("update_rate", update_rate_, 100.0); 19 | params_.Ts = 1.0f/update_rate_; 20 | params_.gravity = 9.8; 21 | nh_private_.param("rho", params_.rho, 1.225); 22 | nh_private_.param("sigma_accel", params_.sigma_accel, 0.0245); 23 | nh_private_.param("sigma_n_gps", params_.sigma_n_gps, 0.21); 24 | nh_private_.param("sigma_e_gps", params_.sigma_e_gps, 0.21); 25 | nh_private_.param("sigma_Vg_gps", params_.sigma_Vg_gps, 0.0500); 26 | nh_private_.param("sigma_couse_gps", params_.sigma_course_gps, 0.0045); 27 | 28 | gnss_fix_sub_ = nh_.subscribe(gnss_fix_topic_, 10, &estimator_base::gnssFixCallback, this); 29 | gnss_vel_sub_ = nh_.subscribe(gnss_vel_topic_, 10, &estimator_base::gnssVelCallback, this); 30 | imu_sub_ = nh_.subscribe(imu_topic_, 10, &estimator_base::imuCallback, this); 31 | baro_sub_ = nh_.subscribe(baro_topic_, 10, &estimator_base::baroAltCallback, this); 32 | airspeed_sub_ = nh_.subscribe(airspeed_topic_, 10, &estimator_base::airspeedCallback, this); 33 | status_sub_ = nh_.subscribe(status_topic_, 1, &estimator_base::statusCallback, this); 34 | update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &estimator_base::update, this); 35 | vehicle_state_pub_ = nh_.advertise("state", 10); 36 | init_static_ = 0; 37 | baro_count_ = 0; 38 | armed_first_time_ = false; 39 | } 40 | 41 | void estimator_base::update(const ros::TimerEvent &) 42 | { 43 | struct output_s output; 44 | 45 | if (armed_first_time_) 46 | { 47 | estimate(params_, input_, output); 48 | } 49 | else 50 | { 51 | output.pn = output.pe = output.h = 0; 52 | output.phi = output.theta = output.psi = 0; 53 | output.alpha = output.beta = output.chi = 0; 54 | output.p = output.q = output.r = 0; 55 | output.Va = 0; 56 | } 57 | 58 | input_.gps_new = false; 59 | 60 | rosplane_msgs::State msg; 61 | msg.header.stamp = ros::Time::now(); 62 | msg.header.frame_id = 1; // Denotes global frame 63 | 64 | msg.position[0] = output.pn; 65 | msg.position[1] = output.pe; 66 | msg.position[2] = -output.h; 67 | if (gps_init_) 68 | { 69 | msg.initial_lat = init_lat_; 70 | msg.initial_lon = init_lon_; 71 | msg.initial_alt = init_alt_; 72 | } 73 | msg.Va = output.Va; 74 | msg.alpha = output.alpha; 75 | msg.beta = output.beta; 76 | msg.phi = output.phi; 77 | msg.theta = output.theta; 78 | msg.psi = output.psi; 79 | msg.chi = output.chi; 80 | msg.p = output.p; 81 | msg.q = output.q; 82 | msg.r = output.r; 83 | msg.Vg = output.Vg; 84 | msg.wn = output.wn; 85 | msg.we = output.we; 86 | msg.quat_valid = false; 87 | 88 | msg.psi_deg = fmod(output.psi, 2.0*M_PI)*180/M_PI; //-360 to 360 89 | msg.psi_deg += (msg.psi_deg < -180 ? 360 : 0); 90 | msg.psi_deg -= (msg.psi_deg > 180 ? 360 : 0); 91 | msg.chi_deg = fmod(output.chi, 2.0*M_PI)*180/M_PI; //-360 to 360 92 | msg.chi_deg += (msg.chi_deg < -180 ? 360 : 0); 93 | msg.chi_deg -= (msg.chi_deg > 180 ? 360 : 0); 94 | 95 | vehicle_state_pub_.publish(msg); 96 | } 97 | 98 | void estimator_base::gnssFixCallback(const sensor_msgs::NavSatFix &msg) 99 | { 100 | bool has_fix = msg.status.status >= sensor_msgs::NavSatStatus::STATUS_FIX; // Higher values refer to augmented fixes 101 | if (!has_fix || !std::isfinite(msg.latitude)) 102 | { 103 | input_.gps_new = false; 104 | return; 105 | } 106 | if (!gps_init_) 107 | { 108 | gps_init_ = true; 109 | init_alt_ = msg.altitude; 110 | init_lat_ = msg.latitude; 111 | init_lon_ = msg.longitude; 112 | } 113 | else 114 | { 115 | input_.gps_n = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0; 116 | input_.gps_e = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0; 117 | input_.gps_h = msg.altitude - init_alt_; 118 | input_.gps_new = true; 119 | } 120 | } 121 | void estimator_base::gnssVelCallback(const geometry_msgs::TwistStamped &msg) 122 | { 123 | double v_n = msg.twist.linear.x; 124 | double v_e = msg.twist.linear.y; 125 | double v_d = msg.twist.linear.z; 126 | double ground_speed = sqrt(v_n * v_n + v_e * v_e); 127 | double course = atan2(v_e, v_n); //Does this need to be in a specific range? All uses seem to accept anything. 128 | input_.gps_Vg = ground_speed; 129 | if(ground_speed > 0.3) //this is a magic number. What is it determined from? 130 | input_.gps_course = course; 131 | } 132 | 133 | void estimator_base::imuCallback(const sensor_msgs::Imu &msg) 134 | { 135 | input_.accel_x = msg.linear_acceleration.x; 136 | input_.accel_y = msg.linear_acceleration.y; 137 | input_.accel_z = msg.linear_acceleration.z; 138 | 139 | input_.gyro_x = msg.angular_velocity.x; 140 | input_.gyro_y = msg.angular_velocity.y; 141 | input_.gyro_z = msg.angular_velocity.z; 142 | } 143 | 144 | void estimator_base::baroAltCallback(const rosflight_msgs::Barometer &msg) 145 | { 146 | 147 | if (armed_first_time_ && !baro_init_) 148 | { 149 | if (baro_count_ < 100) 150 | { 151 | init_static_ += msg.pressure; 152 | init_static_vector_.push_back(msg.pressure); 153 | input_.static_pres = 0; 154 | baro_count_ += 1; 155 | } 156 | else 157 | { 158 | init_static_ = std::accumulate(init_static_vector_.begin(), init_static_vector_.end(), 159 | 0.0)/init_static_vector_.size(); 160 | baro_init_ = true; 161 | 162 | //Check that it got a good calibration. 163 | std::sort(init_static_vector_.begin(), init_static_vector_.end()); 164 | float q1 = (init_static_vector_[24] + init_static_vector_[25])/2.0; 165 | float q3 = (init_static_vector_[74] + init_static_vector_[75])/2.0; 166 | float IQR = q3 - q1; 167 | float upper_bound = q3 + 2.0*IQR; 168 | float lower_bound = q1 - 2.0*IQR; 169 | for (int i = 0; i < 100; i++) 170 | { 171 | if (init_static_vector_[i] > upper_bound) 172 | { 173 | baro_init_ = false; 174 | baro_count_ = 0; 175 | init_static_vector_.clear(); 176 | ROS_WARN("Bad baro calibration. Recalibrating"); 177 | break; 178 | } 179 | else if (init_static_vector_[i] < lower_bound) 180 | { 181 | baro_init_ = false; 182 | baro_count_ = 0; 183 | init_static_vector_.clear(); 184 | ROS_WARN("Bad baro calibration. Recalibrating"); 185 | break; 186 | } 187 | } 188 | } 189 | } 190 | else 191 | { 192 | float static_pres_old = input_.static_pres; 193 | input_.static_pres = -msg.pressure + init_static_; 194 | 195 | float gate_gain = 1.35*params_.rho*params_.gravity; 196 | if (input_.static_pres < static_pres_old - gate_gain) 197 | { 198 | input_.static_pres = static_pres_old - gate_gain; 199 | } 200 | else if (input_.static_pres > static_pres_old + gate_gain) 201 | { 202 | input_.static_pres = static_pres_old + gate_gain; 203 | } 204 | } 205 | } 206 | 207 | void estimator_base::airspeedCallback(const rosflight_msgs::Airspeed &msg) 208 | { 209 | float diff_pres_old = input_.diff_pres; 210 | input_.diff_pres = msg.differential_pressure; 211 | 212 | float gate_gain = pow(3, 2)*params_.rho/2.0; 213 | if (input_.diff_pres < diff_pres_old - gate_gain) 214 | { 215 | input_.diff_pres = diff_pres_old - gate_gain; 216 | } 217 | else if (input_.diff_pres > diff_pres_old + gate_gain) 218 | { 219 | input_.diff_pres = diff_pres_old + gate_gain; 220 | } 221 | } 222 | 223 | void estimator_base::statusCallback(const rosflight_msgs::Status &msg) 224 | { 225 | if (!armed_first_time_ && msg.armed) 226 | armed_first_time_ = true; 227 | } 228 | 229 | } //end namespace 230 | 231 | int main(int argc, char **argv) 232 | { 233 | ros::init(argc, argv, "rosplane_estimator"); 234 | rosplane::estimator_base *est = new rosplane::estimator_example(); 235 | 236 | ros::spin(); 237 | 238 | return 0; 239 | } 240 | -------------------------------------------------------------------------------- /rosplane/src/estimator_example.cpp: -------------------------------------------------------------------------------- 1 | #include "estimator_base.h" 2 | #include "estimator_example.h" 3 | 4 | namespace rosplane 5 | { 6 | 7 | float radians(float degrees) 8 | { 9 | return M_PI*degrees/180.0; 10 | } 11 | 12 | estimator_example::estimator_example() : 13 | estimator_base(), 14 | xhat_a_(Eigen::Vector2f::Zero()), 15 | P_a_(Eigen::Matrix2f::Identity()), 16 | Q_a_(Eigen::Matrix2f::Identity()), 17 | xhat_p_(Eigen::VectorXf::Zero(7)), 18 | P_p_(Eigen::MatrixXf::Identity(7, 7)), 19 | Q_p_(Eigen::MatrixXf::Identity(7, 7)), 20 | R_p_(Eigen::MatrixXf::Zero(7, 7)), 21 | f_p_(7), 22 | A_p_(7, 7), 23 | C_p_(7), 24 | L_p_(7) 25 | { 26 | P_a_ *= powf(radians(5.0f), 2); 27 | 28 | Q_a_(0, 0) = 0.00000001; 29 | Q_a_(1, 1) = 0.00000001; 30 | 31 | P_p_ = Eigen::MatrixXf::Identity(7, 7); 32 | P_p_(0, 0) = .03; 33 | P_p_(1, 1) = .03; 34 | P_p_(2, 2) = .01; 35 | P_p_(3, 3) = radians(5.0f); 36 | P_p_(4, 4) = .04; 37 | P_p_(5, 5) = .04; 38 | P_p_(6, 6) = radians(5.0f); 39 | 40 | Q_p_ *= 0.0001f; 41 | Q_p_(3, 3) = 0.000001f; 42 | 43 | phat_ = 0; 44 | qhat_ = 0; 45 | rhat_ = 0; 46 | phihat_ = 0; 47 | thetahat_ = 0; 48 | psihat_ = 0; 49 | Vwhat_ = 0; 50 | 51 | lpf_static_ = 0; 52 | lpf_diff_ = 0; 53 | 54 | N_ = 10; 55 | 56 | alpha_ = 0.0f; 57 | } 58 | 59 | void estimator_example::estimate(const params_s ¶ms, const input_s &input, output_s &output) 60 | { 61 | if (alpha_ == 0.0f) //initailze stuff that comes from params 62 | { 63 | R_accel_ = powf(params.sigma_accel, 2); 64 | 65 | R_p_(0, 0) = powf(params.sigma_n_gps, 2); 66 | R_p_(1, 1) = powf(params.sigma_e_gps, 2); 67 | R_p_(2, 2) = powf(params.sigma_Vg_gps, 2); 68 | R_p_(3, 3) = powf(params.sigma_course_gps, 2); 69 | R_p_(4, 4) = 0.001; 70 | R_p_(5, 5) = 0.001; 71 | 72 | float lpf_a = 50.0; 73 | float lpf_a1 = 8.0; 74 | alpha_ = exp(-lpf_a*params.Ts); 75 | alpha1_ = exp(-lpf_a1*params.Ts); 76 | } 77 | 78 | // low pass filter gyros to estimate angular rates 79 | lpf_gyro_x_ = alpha_*lpf_gyro_x_ + (1 - alpha_)*input.gyro_x; 80 | lpf_gyro_y_ = alpha_*lpf_gyro_y_ + (1 - alpha_)*input.gyro_y; 81 | lpf_gyro_z_ = alpha_*lpf_gyro_z_ + (1 - alpha_)*input.gyro_z; 82 | 83 | float phat = lpf_gyro_x_; 84 | float qhat = lpf_gyro_y_; 85 | float rhat = lpf_gyro_z_; 86 | 87 | // low pass filter static pressure sensor and invert to esimate altitude 88 | lpf_static_ = alpha1_*lpf_static_ + (1 - alpha1_)*input.static_pres; 89 | float hhat = lpf_static_/params.rho/params.gravity; 90 | 91 | // low pass filter diff pressure sensor and invert to extimate Va 92 | lpf_diff_ = alpha1_*lpf_diff_ + (1 - alpha1_)*input.diff_pres; 93 | 94 | // when the plane isn't moving or moving slowly, the noise in the sensor 95 | // will cause the differential pressure to go negative. This will catch 96 | // those cases. 97 | if (lpf_diff_ <= 0) 98 | lpf_diff_ = 0.000001; 99 | 100 | float Vahat = sqrt(2/params.rho*lpf_diff_); 101 | 102 | // low pass filter accelerometers 103 | lpf_accel_x_ = alpha_*lpf_accel_x_ + (1 - alpha_)*input.accel_x; 104 | lpf_accel_y_ = alpha_*lpf_accel_y_ + (1 - alpha_)*input.accel_y; 105 | lpf_accel_z_ = alpha_*lpf_accel_z_ + (1 - alpha_)*input.accel_z; 106 | 107 | // implement continuous-discrete EKF to estimate roll and pitch angles 108 | // prediction step 109 | float cp; // cos(phi) 110 | float sp; // sin(phi) 111 | float tt; // tan(thata) 112 | float ct; // cos(thata) 113 | float st; // sin(theta) 114 | for (int i = 0; i < N_; i++) 115 | { 116 | cp = cosf(xhat_a_(0)); // cos(phi) 117 | sp = sinf(xhat_a_(0)); // sin(phi) 118 | tt = tanf(xhat_a_(1)); // tan(thata) 119 | ct = cosf(xhat_a_(1)); // cos(thata) 120 | 121 | f_a_(0) = phat + (qhat*sp + rhat*cp)*tt; 122 | f_a_(1) = qhat*cp - rhat*sp; 123 | 124 | A_a_ = Eigen::Matrix2f::Zero(); 125 | A_a_(0, 0) = (qhat*cp - rhat*sp)*tt; 126 | A_a_(0, 1) = (qhat*sp + rhat*cp)/ct/ct; 127 | A_a_(1, 0) = -qhat*sp - rhat*cp; 128 | 129 | xhat_a_ += f_a_*(params.Ts/N_); 130 | P_a_ += (A_a_*P_a_ + P_a_*A_a_.transpose() + Q_a_)*(params.Ts/N_); 131 | } 132 | // measurement updates 133 | cp = cosf(xhat_a_(0)); 134 | sp = sinf(xhat_a_(0)); 135 | ct = cosf(xhat_a_(1)); 136 | st = sinf(xhat_a_(1)); 137 | Eigen::Matrix2f I; 138 | I = Eigen::Matrix2f::Identity(); 139 | 140 | // x-axis accelerometer 141 | h_a_ = qhat*Vahat*st + params.gravity*st; 142 | C_a_ = Eigen::Vector2f::Zero(); 143 | C_a_(1) = qhat*Vahat*ct + params.gravity*ct; 144 | L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_); 145 | P_a_ = (I - L_a_*C_a_.transpose())*P_a_; 146 | xhat_a_ += L_a_*((hhat < 15 ? lpf_accel_x_/3 : lpf_accel_x_) - h_a_); 147 | 148 | // y-axis accelerometer 149 | h_a_ = rhat*Vahat*ct - phat*Vahat*st - params.gravity*ct*sp; 150 | C_a_ = Eigen::Vector2f::Zero(); 151 | C_a_(0) = -params.gravity*cp*ct; 152 | C_a_(1) = -rhat*Vahat*st - phat*Vahat*ct + params.gravity*st*sp; 153 | L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_); 154 | P_a_ = (I - L_a_*C_a_.transpose())*P_a_; 155 | xhat_a_ += L_a_*(lpf_accel_y_ - h_a_); 156 | 157 | // z-axis accelerometer 158 | h_a_ = -qhat*Vahat*ct - params.gravity*ct*cp; 159 | C_a_ = Eigen::Vector2f::Zero(); 160 | C_a_(0) = params.gravity*sp*ct; 161 | C_a_(1) = (qhat*Vahat + params.gravity*cp)*st; 162 | L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_); 163 | P_a_ = (I - L_a_*C_a_.transpose())*P_a_; 164 | xhat_a_ += L_a_*(lpf_accel_z_ - h_a_); 165 | 166 | check_xhat_a(); 167 | 168 | float phihat = xhat_a_(0); 169 | float thetahat = xhat_a_(1); 170 | 171 | // implement continous-discrete EKF to estimate pn, pe, chi, Vg 172 | // prediction step 173 | float psidot, tmp, Vgdot; 174 | if (fabsf(xhat_p_(2)) < 0.01f) 175 | { 176 | xhat_p_(2) = 0.01; // prevent devide by zero 177 | } 178 | 179 | for (int i = 0; i < N_; i++) 180 | { 181 | psidot = (qhat*sinf(phihat) + rhat*cosf(phihat))/cosf(thetahat); 182 | tmp = -psidot*Vahat*(xhat_p_(4)*cosf(xhat_p_(6)) + xhat_p_(5)*sinf(xhat_p_(6)))/xhat_p_(2); 183 | Vgdot = ((Vahat*cosf(xhat_p_(6)) + xhat_p_(4))*(-psidot*Vahat*sinf(xhat_p_(6))) + (Vahat*sinf(xhat_p_( 184 | 6)) + xhat_p_(5))*(psidot*Vahat*cosf(xhat_p_(6))))/xhat_p_(2); 185 | 186 | f_p_ = Eigen::VectorXf::Zero(7); 187 | f_p_(0) = xhat_p_(2)*cosf(xhat_p_(3)); 188 | f_p_(1) = xhat_p_(2)*sinf(xhat_p_(3)); 189 | f_p_(2) = Vgdot; 190 | f_p_(3) = params.gravity/xhat_p_(2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6)); 191 | f_p_(6) = psidot; 192 | 193 | A_p_ = Eigen::MatrixXf::Zero(7, 7); 194 | A_p_(0, 2) = cos(xhat_p_(3)); 195 | A_p_(0, 3) = -xhat_p_(2)*sinf(xhat_p_(3)); 196 | A_p_(1, 2) = sin(xhat_p_(3)); 197 | A_p_(1, 3) = xhat_p_(2)*cosf(xhat_p_(3)); 198 | A_p_(2, 2) = -Vgdot/xhat_p_(2); 199 | A_p_(2, 4) = -psidot*Vahat*sinf(xhat_p_(6))/xhat_p_(2); 200 | A_p_(2, 5) = psidot*Vahat*cosf(xhat_p_(6))/xhat_p_(2); 201 | A_p_(2, 6) = tmp; 202 | A_p_(3, 2) = -params.gravity/powf(xhat_p_(2), 2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6)); 203 | A_p_(3, 3) = -params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6)); 204 | A_p_(3, 6) = params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6)); 205 | 206 | xhat_p_ += f_p_*(params.Ts/N_); 207 | P_p_ += (A_p_*P_p_ + P_p_*A_p_.transpose() + Q_p_)*(params.Ts/N_); 208 | } 209 | 210 | // while(xhat_p(3) > radians(180.0f)) xhat_p(3) = xhat_p(3) - radians(360.0f); 211 | // while(xhat_p(3) < radians(-180.0f)) xhat_p(3) = xhat_p(3) + radians(360.0f); 212 | // if(xhat_p(3) > radians(180.0f) || xhat_p(3) < radians(-180.0f)) 213 | // { 214 | // ROS_WARN("Course estimate not wrapped from -pi to pi"); 215 | // xhat_p(3) = 0; 216 | // } 217 | 218 | // measurement updates 219 | if (input.gps_new) 220 | { 221 | Eigen::MatrixXf I_p(7, 7); 222 | I_p = Eigen::MatrixXf::Identity(7, 7); 223 | 224 | // gps North position 225 | h_p_ = xhat_p_(0); 226 | C_p_ = Eigen::VectorXf::Zero(7); 227 | C_p_(0) = 1; 228 | L_p_ = (P_p_*C_p_)/(R_p_(0, 0) + (C_p_.transpose()*P_p_*C_p_)); 229 | P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; 230 | xhat_p_ = xhat_p_ + L_p_*(input.gps_n - h_p_); 231 | 232 | // gps East position 233 | h_p_ = xhat_p_(1); 234 | C_p_ = Eigen::VectorXf::Zero(7); 235 | C_p_(1) = 1; 236 | L_p_ = (P_p_*C_p_)/(R_p_(1, 1) + (C_p_.transpose()*P_p_*C_p_)); 237 | P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; 238 | xhat_p_ = xhat_p_ + L_p_*(input.gps_e - h_p_); 239 | 240 | // gps ground speed 241 | h_p_ = xhat_p_(2); 242 | C_p_ = Eigen::VectorXf::Zero(7); 243 | C_p_(2) = 1; 244 | L_p_ = (P_p_*C_p_)/(R_p_(2, 2) + (C_p_.transpose()*P_p_*C_p_)); 245 | P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; 246 | xhat_p_ = xhat_p_ + L_p_*(input.gps_Vg - h_p_); 247 | 248 | // gps course 249 | //wrap course measurement 250 | float gps_course = fmodf(input.gps_course, radians(360.0f)); 251 | 252 | while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f); 253 | while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f); 254 | h_p_ = xhat_p_(3); 255 | C_p_ = Eigen::VectorXf::Zero(7); 256 | C_p_(3) = 1; 257 | L_p_ = (P_p_*C_p_)/(R_p_(3, 3) + (C_p_.transpose()*P_p_*C_p_)); 258 | P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_; 259 | xhat_p_ = xhat_p_ + L_p_*(gps_course - h_p_); 260 | 261 | // // pseudo measurement #1 y_1 = Va*cos(psi)+wn-Vg*cos(chi) 262 | // h_p = Vahat*cosf(xhat_p(6)) + xhat_p(4) - xhat_p(2)*cosf(xhat_p(3)); // pseudo measurement 263 | // C_p = Eigen::VectorXf::Zero(7); 264 | // C_p(2) = -cos(xhat_p(3)); 265 | // C_p(3) = xhat_p(2)*sinf(xhat_p(3)); 266 | // C_p(4) = 1; 267 | // C_p(6) = -Vahat*sinf(xhat_p(6)); 268 | // L_p = (P_p*C_p)/(R_p(4,4) + (C_p.transpose()*P_p*C_p)); 269 | // P_p = (I_p - L_p*C_p.transpose())*P_p; 270 | // xhat_p = xhat_p + L_p*(0 - h_p); 271 | 272 | // // pseudo measurement #2 y_2 = Va*sin(psi) + we - Vg*sin(chi) 273 | // h_p = Vahat*sinf(xhat_p(6))+xhat_p(5)-xhat_p(2)*sinf(xhat_p(3)); // pseudo measurement 274 | // C_p = Eigen::VectorXf::Zero(7); 275 | // C_p(2) = -sin(xhat_p(3)); 276 | // C_p(3) = -xhat_p(2)*cosf(xhat_p(3)); 277 | // C_p(5) = 1; 278 | // C_p(6) = Vahat*cosf(xhat_p(6)); 279 | // L_p = (P_p*C_p)/(R_p(5,5) + (C_p.transpose()*P_p*C_p)); 280 | // P_p = (I_p - L_p*C_p.transpose())*P_p; 281 | // xhat_p = xhat_p + L_p*(0 - h_p); 282 | 283 | if (xhat_p_(0) > 10000 || xhat_p_(0) < -10000) 284 | { 285 | ROS_WARN("gps n limit reached"); 286 | xhat_p_(0) = input.gps_n; 287 | } 288 | if (xhat_p_(1) > 10000 || xhat_p_(1) < -10000) 289 | { 290 | ROS_WARN("gps e limit reached"); 291 | xhat_p_(1) = input.gps_e; 292 | } 293 | } 294 | 295 | bool problem = false; 296 | int prob_index; 297 | for (int i = 0; i < 7; i++) 298 | { 299 | if (!std::isfinite(xhat_p_(i))) 300 | { 301 | if (!problem) 302 | { 303 | problem = true; 304 | prob_index = i; 305 | } 306 | switch (i) 307 | { 308 | case 0: 309 | xhat_p_(i) = input.gps_n; 310 | break; 311 | case 1: 312 | xhat_p_(i) = input.gps_e; 313 | break; 314 | case 2: 315 | xhat_p_(i) = input.gps_Vg; 316 | break; 317 | case 3: 318 | xhat_p_(i) = input.gps_course; 319 | break; 320 | case 6: 321 | xhat_p_(i) = input.gps_course; 322 | break; 323 | default: 324 | xhat_p_(i) = 0; 325 | } 326 | P_p_ = Eigen::MatrixXf::Identity(7, 7); 327 | P_p_(0, 0) = .03; 328 | P_p_(1, 1) = .03; 329 | P_p_(2, 2) = .01; 330 | P_p_(3, 3) = radians(5.0f); 331 | P_p_(4, 4) = .04; 332 | P_p_(5, 5) = .04; 333 | P_p_(6, 6) = radians(5.0f); 334 | } 335 | } 336 | if (problem) 337 | { 338 | ROS_WARN("position estimator reinitialized due to non-finite state %d", prob_index); 339 | } 340 | if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f)) 341 | { 342 | //xhat_p(3) = fmodf(xhat_p(3),2.0*M_PI); 343 | xhat_p_(6) = fmodf(xhat_p_(6), 2.0*M_PI); 344 | } 345 | 346 | float pnhat = xhat_p_(0); 347 | float pehat = xhat_p_(1); 348 | float Vghat = xhat_p_(2); 349 | float chihat = xhat_p_(3); 350 | float wnhat = xhat_p_(4); 351 | float wehat = xhat_p_(5); 352 | float psihat = xhat_p_(6); 353 | 354 | output.pn = pnhat; 355 | output.pe = pehat; 356 | output.h = hhat; 357 | output.Va = Vahat; 358 | output.alpha = 0; 359 | output.beta = 0; 360 | output.phi = phihat; 361 | output.theta = thetahat; 362 | output.chi = chihat; 363 | output.p = phat; 364 | output.q = qhat; 365 | output.r = rhat; 366 | output.Vg = Vghat; 367 | output.wn = wnhat; 368 | output.we = wehat; 369 | output.psi = psihat; 370 | } 371 | 372 | void estimator_example::check_xhat_a() 373 | { 374 | if (xhat_a_(0) > radians(85.0) || xhat_a_(0) < radians(-85.0) || !std::isfinite(xhat_a_(0))) 375 | { 376 | if (!std::isfinite(xhat_a_(0))) 377 | { 378 | xhat_a_(0) = 0; 379 | P_a_ = Eigen::Matrix2f::Identity(); 380 | P_a_ *= powf(radians(20.0f), 2); 381 | ROS_WARN("attiude estimator reinitialized due to non-finite roll"); 382 | } 383 | else if (xhat_a_(0) > radians(85.0)) 384 | { 385 | xhat_a_(0) = radians(82.0); 386 | ROS_WARN("max roll angle"); 387 | } 388 | else if (xhat_a_(0) < radians(-85.0)) 389 | { 390 | xhat_a_(0) = radians(-82.0); 391 | ROS_WARN("min roll angle"); 392 | } 393 | } 394 | if (xhat_a_(1) > radians(80.0) || xhat_a_(1) < radians(-80.0) || !std::isfinite(xhat_a_(1))) 395 | { 396 | if (!std::isfinite(xhat_a_(1))) 397 | { 398 | xhat_a_(1) = 0; 399 | P_a_ = Eigen::Matrix2f::Identity(); 400 | P_a_ *= powf(radians(20.0f), 2); 401 | ROS_WARN("attiude estimator reinitialized due to non-finite pitch"); 402 | } 403 | else if (xhat_a_(1) > radians(80.0)) 404 | { 405 | xhat_a_(1) = radians(77.0); 406 | ROS_WARN("max pitch angle"); 407 | } 408 | else if (xhat_a_(1) < radians(-80.0)) 409 | { 410 | xhat_a_(1) = radians(-77.0); 411 | ROS_WARN("min pitch angle"); 412 | } 413 | } 414 | } 415 | 416 | } //end namespace 417 | -------------------------------------------------------------------------------- /rosplane/src/path_follower_base.cpp: -------------------------------------------------------------------------------- 1 | #include "path_follower_base.h" 2 | #include "path_follower_example.h" 3 | 4 | namespace rosplane 5 | { 6 | 7 | path_follower_base::path_follower_base(): 8 | nh_(ros::NodeHandle()), 9 | nh_private_(ros::NodeHandle("~")) 10 | { 11 | vehicle_state_sub_ = nh_.subscribe("state", 1, &path_follower_base::vehicle_state_callback, this); 12 | current_path_sub_ = nh_.subscribe("current_path", 1, 13 | &path_follower_base::current_path_callback, this); 14 | 15 | 16 | nh_private_.param("CHI_INFTY", params_.chi_infty, 1.0472); 17 | nh_private_.param("K_PATH", params_.k_path, 0.025); 18 | nh_private_.param("K_ORBIT", params_.k_orbit, 4.0); 19 | 20 | func_ = boost::bind(&path_follower_base::reconfigure_callback, this, _1, _2); 21 | server_.setCallback(func_); 22 | 23 | update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &path_follower_base::update, this); 24 | controller_commands_pub_ = nh_.advertise("controller_commands", 1); 25 | 26 | state_init_ = false; 27 | current_path_init_ = false; 28 | } 29 | 30 | void path_follower_base::update(const ros::TimerEvent &) 31 | { 32 | 33 | struct output_s output; 34 | 35 | if (state_init_ == true && current_path_init_ == true) 36 | { 37 | follow(params_, input_, output); 38 | rosplane_msgs::Controller_Commands msg; 39 | msg.chi_c = output.chi_c; 40 | msg.Va_c = output.Va_c; 41 | msg.h_c = output.h_c; 42 | msg.phi_ff = output.phi_ff; 43 | controller_commands_pub_.publish(msg); 44 | } 45 | } 46 | 47 | void path_follower_base::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg) 48 | { 49 | input_.pn = msg->position[0]; /** position north */ 50 | input_.pe = msg->position[1]; /** position east */ 51 | input_.h = -msg->position[2]; /** altitude */ 52 | input_.chi = msg->chi; 53 | input_.Va = msg->Va; 54 | 55 | state_init_ = true; 56 | } 57 | 58 | void path_follower_base::current_path_callback(const rosplane_msgs::Current_PathConstPtr &msg) 59 | { 60 | if (msg->path_type == msg->LINE_PATH) 61 | input_.p_type = path_type::Line; 62 | else if (msg->path_type == msg->ORBIT_PATH) 63 | input_.p_type = path_type::Orbit; 64 | 65 | input_.Va_d = msg->Va_d; 66 | for (int i = 0; i < 3; i++) 67 | { 68 | input_.r_path[i] = msg->r[i]; 69 | input_.q_path[i] = msg->q[i]; 70 | input_.c_orbit[i] = msg->c[i]; 71 | } 72 | input_.rho_orbit = msg->rho; 73 | input_.lam_orbit = msg->lambda; 74 | current_path_init_ = true; 75 | } 76 | 77 | void path_follower_base::reconfigure_callback(rosplane::FollowerConfig &config, uint32_t level) 78 | { 79 | params_.chi_infty = config.CHI_INFTY; 80 | params_.k_path = config.K_PATH; 81 | params_.k_orbit = config.K_ORBIT; 82 | } 83 | } //end namespace 84 | 85 | int main(int argc, char **argv) 86 | { 87 | ros::init(argc, argv, "rosplane_path_follower"); 88 | rosplane::path_follower_base *path = new rosplane::path_follower_example(); 89 | 90 | ros::spin(); 91 | 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /rosplane/src/path_follower_example.cpp: -------------------------------------------------------------------------------- 1 | #include "path_follower_example.h" 2 | 3 | 4 | namespace rosplane 5 | { 6 | 7 | path_follower_example::path_follower_example() 8 | { 9 | } 10 | 11 | void path_follower_example::follow(const params_s ¶ms, const input_s &input, output_s &output) 12 | { 13 | if (input.p_type == path_type::Line) // follow straight line path specified by r and q 14 | { 15 | // compute wrapped version of the path angle 16 | float chi_q = atan2f(input.q_path[1], input.q_path[0]); 17 | while (chi_q - input.chi < -M_PI) 18 | chi_q += 2.0*M_PI; 19 | while (chi_q - input.chi > M_PI) 20 | chi_q -= 2.0*M_PI; 21 | 22 | float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]); 23 | // heading command 24 | output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params.k_path*path_error); 25 | 26 | // desired altitude 27 | float h_d = -input.r_path[2] - sqrtf(powf((input.r_path[0] - input.pn), 2) + powf((input.r_path[1] - input.pe), 28 | 2))*(input.q_path[2])/sqrtf(powf(input.q_path[0], 2) + powf(input.q_path[1], 2)); 29 | // commanded altitude is desired altitude 30 | output.h_c = h_d; 31 | output.phi_ff = 0.0; 32 | } 33 | else // follow a orbit path specified by c_orbit, rho_orbit, and lam_orbit 34 | { 35 | float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + powf((input.pe - input.c_orbit[1]), 36 | 2)); // distance from orbit center 37 | // compute wrapped version of angular position on orbit 38 | float varphi = atan2f(input.pe - input.c_orbit[1], input.pn - input.c_orbit[0]); 39 | while ((varphi - input.chi) < -M_PI) 40 | varphi += 2.0*M_PI; 41 | while ((varphi - input.chi) > M_PI) 42 | varphi -= 2.0*M_PI; 43 | //compute orbit error 44 | float norm_orbit_error = (d - input.rho_orbit)/input.rho_orbit; 45 | output.chi_c = varphi + input.lam_orbit*(M_PI/2.0 + atanf(params.k_orbit*norm_orbit_error)); 46 | 47 | // commanded altitude is the height of the orbit 48 | float h_d = -input.c_orbit[2]; 49 | output.h_c = h_d; 50 | output.phi_ff = (norm_orbit_error < 0.5 ? input.lam_orbit*atanf(input.Va*input.Va/(9.8*input.rho_orbit)) : 0); 51 | } 52 | output.Va_c = input.Va_d; 53 | } 54 | 55 | } //end namespace 56 | -------------------------------------------------------------------------------- /rosplane/src/path_manager_base.cpp: -------------------------------------------------------------------------------- 1 | #include "path_manager_base.h" 2 | #include "path_manager_example.h" 3 | 4 | namespace rosplane 5 | { 6 | 7 | path_manager_base::path_manager_base(): 8 | nh_(ros::NodeHandle()), /** nh_ stuff added here */ 9 | nh_private_(ros::NodeHandle("~")) 10 | { 11 | nh_private_.param("R_min", params_.R_min, 25.0); 12 | nh_private_.param("update_rate", update_rate_, 10.0); 13 | 14 | vehicle_state_sub_ = nh_.subscribe("state", 10, &path_manager_base::vehicle_state_callback, this); 15 | new_waypoint_sub_ = nh_.subscribe("waypoint_path", 10, &path_manager_base::new_waypoint_callback, this); 16 | current_path_pub_ = nh_.advertise("current_path", 10); 17 | 18 | update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &path_manager_base::current_path_publish, this); 19 | 20 | num_waypoints_ = 0; 21 | 22 | state_init_ = false; 23 | } 24 | 25 | void path_manager_base::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg) 26 | { 27 | vehicle_state_ = *msg; 28 | 29 | state_init_ = true; 30 | } 31 | 32 | void path_manager_base::new_waypoint_callback(const rosplane_msgs::Waypoint &msg) 33 | { 34 | if (msg.clear_wp_list == true) 35 | { 36 | waypoints_.clear(); 37 | num_waypoints_ = 0; 38 | idx_a_ = 0; 39 | return; 40 | } 41 | if (msg.set_current || num_waypoints_ == 0) 42 | { 43 | waypoint_s currentwp; 44 | currentwp.w[0] = vehicle_state_.position[0]; 45 | currentwp.w[1] = vehicle_state_.position[1]; 46 | currentwp.w[2] = (vehicle_state_.position[2] > -25 ? msg.w[2] : vehicle_state_.position[2]); 47 | currentwp.chi_d = vehicle_state_.chi; 48 | currentwp.chi_valid = msg.chi_valid; 49 | currentwp.Va_d = msg.Va_d; 50 | 51 | waypoints_.clear(); 52 | waypoints_.push_back(currentwp); 53 | num_waypoints_ = 1; 54 | idx_a_ = 0; 55 | } 56 | waypoint_s nextwp; 57 | nextwp.w[0] = msg.w[0]; 58 | nextwp.w[1] = msg.w[1]; 59 | nextwp.w[2] = msg.w[2]; 60 | nextwp.chi_d = msg.chi_d; 61 | nextwp.chi_valid = msg.chi_valid; 62 | nextwp.Va_d = msg.Va_d; 63 | waypoints_.push_back(nextwp); 64 | num_waypoints_++; 65 | } 66 | 67 | void path_manager_base::current_path_publish(const ros::TimerEvent &) 68 | { 69 | 70 | struct input_s input; 71 | input.pn = vehicle_state_.position[0]; /** position north */ 72 | input.pe = vehicle_state_.position[1]; /** position east */ 73 | input.h = -vehicle_state_.position[2]; /** altitude */ 74 | input.chi = vehicle_state_.chi; 75 | 76 | struct output_s output; 77 | 78 | if (state_init_ == true) 79 | { 80 | manage(params_, input, output); 81 | } 82 | 83 | rosplane_msgs::Current_Path current_path; 84 | 85 | if (output.flag) 86 | current_path.path_type = current_path.LINE_PATH; 87 | else 88 | current_path.path_type = current_path.ORBIT_PATH; 89 | current_path.Va_d = output.Va_d; 90 | for (int i = 0; i < 3; i++) 91 | { 92 | current_path.r[i] = output.r[i]; 93 | current_path.q[i] = output.q[i]; 94 | current_path.c[i] = output.c[i]; 95 | } 96 | current_path.rho = output.rho; 97 | current_path.lambda = output.lambda; 98 | 99 | current_path_pub_.publish(current_path); 100 | } 101 | 102 | } //end namespace 103 | 104 | int main(int argc, char **argv) 105 | { 106 | ros::init(argc, argv, "rosplane_path_manager"); 107 | rosplane::path_manager_base *est = new rosplane::path_manager_example(); 108 | 109 | ros::spin(); 110 | 111 | return 0; 112 | } 113 | -------------------------------------------------------------------------------- /rosplane/src/path_manager_example.cpp: -------------------------------------------------------------------------------- 1 | #include "path_manager_example.h" 2 | #include "ros/ros.h" 3 | #include 4 | 5 | namespace rosplane 6 | { 7 | 8 | path_manager_example::path_manager_example() : path_manager_base() 9 | { 10 | fil_state_ = fillet_state::STRAIGHT; 11 | dub_state_ = dubin_state::FIRST; 12 | } 13 | 14 | void path_manager_example::manage(const params_s ¶ms, const input_s &input, output_s &output) 15 | { 16 | 17 | if (num_waypoints_ < 2) 18 | { 19 | ROS_WARN_THROTTLE(4, "No waypoints received! Loitering about origin at 50m"); 20 | output.flag = false; 21 | output.Va_d = 12; 22 | output.c[0] = 0.0f; 23 | output.c[1] = 0.0f; 24 | output.c[2] = -50.0f; 25 | output.rho = params.R_min; 26 | output.lambda = 1; 27 | } 28 | else 29 | { 30 | if (waypoints_[idx_a_].chi_valid) 31 | { 32 | manage_dubins(params, input, output); 33 | } 34 | else 35 | { 36 | /** Switch the following for flying directly to waypoints, or filleting corners */ 37 | //manage_line(params, input, output); 38 | manage_fillet(params, input, output); 39 | } 40 | } 41 | } 42 | 43 | void path_manager_example::manage_line(const params_s ¶ms, const input_s &input, output_s &output) 44 | { 45 | 46 | Eigen::Vector3f p; 47 | p << input.pn, input.pe, -input.h; 48 | 49 | int idx_b; 50 | int idx_c; 51 | if (idx_a_ == num_waypoints_ - 1) 52 | { 53 | idx_b = 0; 54 | idx_c = 1; 55 | } 56 | else if (idx_a_ == num_waypoints_ - 2) 57 | { 58 | idx_b = num_waypoints_ - 1; 59 | idx_c = 0; 60 | } 61 | else 62 | { 63 | idx_b = idx_a_ + 1; 64 | idx_c = idx_b + 1; 65 | } 66 | 67 | Eigen::Vector3f w_im1(waypoints_[idx_a_].w); 68 | Eigen::Vector3f w_i(waypoints_[idx_b].w); 69 | Eigen::Vector3f w_ip1(waypoints_[idx_c].w); 70 | 71 | output.flag = true; 72 | output.Va_d = waypoints_[idx_a_].Va_d; 73 | output.r[0] = w_im1(0); 74 | output.r[1] = w_im1(1); 75 | output.r[2] = w_im1(2); 76 | Eigen::Vector3f q_im1 = (w_i - w_im1).normalized(); 77 | Eigen::Vector3f q_i = (w_ip1 - w_i).normalized(); 78 | output.q[0] = q_im1(0); 79 | output.q[1] = q_im1(1); 80 | output.q[2] = q_im1(2); 81 | 82 | Eigen::Vector3f n_i = (q_im1 + q_i).normalized(); 83 | if ((p - w_i).dot(n_i) > 0.0f) 84 | { 85 | if (idx_a_ == num_waypoints_ - 1) 86 | idx_a_ = 0; 87 | else 88 | idx_a_++; 89 | } 90 | 91 | } 92 | 93 | void path_manager_example::manage_fillet(const params_s ¶ms, const input_s &input, output_s &output) 94 | { 95 | if (num_waypoints_ < 3) //since it fillets don't make sense between just two points 96 | { 97 | manage_line(params, input, output); 98 | return; 99 | } 100 | 101 | Eigen::Vector3f p; 102 | p << input.pn, input.pe, -input.h; 103 | 104 | int idx_b; 105 | int idx_c; 106 | if (idx_a_ == num_waypoints_ - 1) 107 | { 108 | idx_b = 0; 109 | idx_c = 1; 110 | } 111 | else if (idx_a_ == num_waypoints_ - 2) 112 | { 113 | idx_b = num_waypoints_ - 1; 114 | idx_c = 0; 115 | } 116 | else 117 | { 118 | idx_b = idx_a_ + 1; 119 | idx_c = idx_b + 1; 120 | } 121 | 122 | Eigen::Vector3f w_im1(waypoints_[idx_a_].w); 123 | Eigen::Vector3f w_i(waypoints_[idx_b].w); 124 | Eigen::Vector3f w_ip1(waypoints_[idx_c].w); 125 | 126 | float R_min = params.R_min; 127 | 128 | output.Va_d = waypoints_[idx_a_].Va_d; 129 | output.r[0] = w_im1(0); 130 | output.r[1] = w_im1(1); 131 | output.r[2] = w_im1(2); 132 | Eigen::Vector3f q_im1 = (w_i - w_im1).normalized(); 133 | Eigen::Vector3f q_i = (w_ip1 - w_i).normalized(); 134 | float beta = acosf(-q_im1.dot(q_i)); 135 | 136 | Eigen::Vector3f z; 137 | switch (fil_state_) 138 | { 139 | case fillet_state::STRAIGHT: 140 | output.flag = true; 141 | output.q[0] = q_im1(0); 142 | output.q[1] = q_im1(1); 143 | output.q[2] = q_im1(2); 144 | output.c[0] = 1; 145 | output.c[1] = 1; 146 | output.c[2] = 1; 147 | output.rho = 1; 148 | output.lambda = 1; 149 | z = w_i - q_im1*(R_min/tanf(beta/2.0)); 150 | if ((p - z).dot(q_im1) > 0) 151 | fil_state_ = fillet_state::ORBIT; 152 | break; 153 | case fillet_state::ORBIT: 154 | output.flag = false; 155 | output.q[0] = q_i(0); 156 | output.q[1] = q_i(1); 157 | output.q[2] = q_i(2); 158 | Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized()*(R_min/sinf(beta/2.0)); 159 | output.c[0] = c(0); 160 | output.c[1] = c(1); 161 | output.c[2] = c(2); 162 | output.rho = R_min; 163 | output.lambda = ((q_im1(0)*q_i(1) - q_im1(1)*q_i(0)) > 0 ? 1 : -1); 164 | z = w_i + q_i*(R_min/tanf(beta/2.0)); 165 | if ((p - z).dot(q_i) > 0) 166 | { 167 | if (idx_a_ == num_waypoints_ - 1) 168 | idx_a_ = 0; 169 | else 170 | idx_a_++; 171 | fil_state_ = fillet_state::STRAIGHT; 172 | } 173 | break; 174 | } 175 | } 176 | 177 | void path_manager_example::manage_dubins(const params_s ¶ms, const input_s &input, output_s &output) 178 | { 179 | Eigen::Vector3f p; 180 | p << input.pn, input.pe, -input.h; 181 | 182 | output.Va_d = waypoints_[idx_a_].Va_d; 183 | output.r[0] = 0; 184 | output.r[1] = 0; 185 | output.r[2] = 0; 186 | output.q[0] = 0; 187 | output.q[1] = 0; 188 | output.q[2] = 0; 189 | output.c[0] = 0; 190 | output.c[1] = 0; 191 | output.c[2] = 0; 192 | 193 | switch (dub_state_) 194 | { 195 | case dubin_state::FIRST: 196 | dubinsParameters(waypoints_[0], waypoints_[1], params.R_min); 197 | output.flag = false; 198 | output.c[0] = dubinspath_.cs(0); 199 | output.c[1] = dubinspath_.cs(1); 200 | output.c[2] = dubinspath_.cs(2); 201 | output.rho = dubinspath_.R; 202 | output.lambda = dubinspath_.lams; 203 | if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1 204 | { 205 | dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE; 206 | } 207 | else 208 | { 209 | dub_state_ = dubin_state::BEFORE_H1; 210 | } 211 | break; 212 | case dubin_state::BEFORE_H1: 213 | output.flag = false; 214 | output.c[0] = dubinspath_.cs(0); 215 | output.c[1] = dubinspath_.cs(1); 216 | output.c[2] = dubinspath_.cs(2); 217 | output.rho = dubinspath_.R; 218 | output.lambda = dubinspath_.lams; 219 | if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // entering H1 220 | { 221 | dub_state_ = dubin_state::STRAIGHT; 222 | } 223 | break; 224 | case dubin_state::BEFORE_H1_WRONG_SIDE: 225 | output.flag = false; 226 | output.c[0] = dubinspath_.cs(0); 227 | output.c[1] = dubinspath_.cs(1); 228 | output.c[2] = dubinspath_.cs(2); 229 | output.rho = dubinspath_.R; 230 | output.lambda = dubinspath_.lams; 231 | if ((p - dubinspath_.w1).dot(dubinspath_.q1) < 0) // exit H1 232 | { 233 | dub_state_ = dubin_state::BEFORE_H1; 234 | } 235 | break; 236 | case dubin_state::STRAIGHT: 237 | output.flag = true; 238 | output.r[0] = dubinspath_.w1(0); 239 | output.r[1] = dubinspath_.w1(1); 240 | output.r[2] = dubinspath_.w1(2); 241 | // output.r[0] = dubinspath_.z1(0); 242 | // output.r[1] = dubinspath_.z1(1); 243 | // output.r[2] = dubinspath_.z1(2); 244 | output.q[0] = dubinspath_.q1(0); 245 | output.q[1] = dubinspath_.q1(1); 246 | output.q[2] = dubinspath_.q1(2); 247 | output.rho = 1; 248 | output.lambda = 1; 249 | if ((p - dubinspath_.w2).dot(dubinspath_.q1) >= 0) // entering H2 250 | { 251 | if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // start in H3 252 | { 253 | dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE; 254 | } 255 | else 256 | { 257 | dub_state_ = dubin_state::BEFORE_H3; 258 | } 259 | } 260 | break; 261 | case dubin_state::BEFORE_H3: 262 | output.flag = false; 263 | output.c[0] = dubinspath_.ce(0); 264 | output.c[1] = dubinspath_.ce(1); 265 | output.c[2] = dubinspath_.ce(2); 266 | output.rho = dubinspath_.R; 267 | output.lambda = dubinspath_.lame; 268 | if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // entering H3 269 | { 270 | // increase the waypoint pointer 271 | int idx_b; 272 | if (idx_a_ == num_waypoints_ - 1) 273 | { 274 | idx_a_ = 0; 275 | idx_b = 1; 276 | } 277 | else if (idx_a_ == num_waypoints_ - 2) 278 | { 279 | idx_a_++; 280 | idx_b = 0; 281 | } 282 | else 283 | { 284 | idx_a_++; 285 | idx_b = idx_a_ + 1; 286 | } 287 | 288 | // plan new Dubin's path to next waypoint configuration 289 | dubinsParameters(waypoints_[idx_a_], waypoints_[idx_b], params.R_min); 290 | 291 | //start new path 292 | if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1 293 | { 294 | dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE; 295 | } 296 | else 297 | { 298 | dub_state_ = dubin_state::BEFORE_H1; 299 | } 300 | } 301 | break; 302 | case dubin_state::BEFORE_H3_WRONG_SIDE: 303 | output.flag = false; 304 | output.c[0] = dubinspath_.ce(0); 305 | output.c[1] = dubinspath_.ce(1); 306 | output.c[2] = dubinspath_.ce(2); 307 | output.rho = dubinspath_.R; 308 | output.lambda = dubinspath_.lame; 309 | if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3 310 | { 311 | dub_state_ = dubin_state::BEFORE_H1; 312 | } 313 | break; 314 | } 315 | } 316 | 317 | Eigen::Matrix3f path_manager_example::rotz(float theta) 318 | { 319 | Eigen::Matrix3f R; 320 | R << cosf(theta), -sinf(theta), 0, 321 | sinf(theta), cosf(theta), 0, 322 | 0, 0, 1; 323 | 324 | return R; 325 | } 326 | 327 | float path_manager_example::mo(float in) 328 | { 329 | float val; 330 | if (in > 0) 331 | val = fmod(in, 2.0*M_PI_F); 332 | else 333 | { 334 | float n = floorf(in/2.0/M_PI_F); 335 | val = in - n*2.0*M_PI_F; 336 | } 337 | return val; 338 | } 339 | 340 | void path_manager_example::dubinsParameters(const waypoint_s start_node, const waypoint_s end_node, float R) 341 | { 342 | float ell = sqrtf((start_node.w[0] - end_node.w[0])*(start_node.w[0] - end_node.w[0]) + 343 | (start_node.w[1] - end_node.w[1])*(start_node.w[1] - end_node.w[1])); 344 | if (ell < 2.0*R) 345 | { 346 | ROS_ERROR("The distance between nodes must be larger than 2R."); 347 | } 348 | else 349 | { 350 | dubinspath_.ps(0) = start_node.w[0]; 351 | dubinspath_.ps(1) = start_node.w[1]; 352 | dubinspath_.ps(2) = start_node.w[2]; 353 | dubinspath_.chis = start_node.chi_d; 354 | dubinspath_.pe(0) = end_node.w[0]; 355 | dubinspath_.pe(1) = end_node.w[1]; 356 | dubinspath_.pe(2) = end_node.w[2]; 357 | dubinspath_.chie = end_node.chi_d; 358 | 359 | 360 | Eigen::Vector3f crs = dubinspath_.ps; 361 | crs(0) += R*(cosf(M_PI_2_F)*cosf(dubinspath_.chis) - sinf(M_PI_2_F)*sinf(dubinspath_.chis)); 362 | crs(1) += R*(sinf(M_PI_2_F)*cosf(dubinspath_.chis) + cosf(M_PI_2_F)*sinf(dubinspath_.chis)); 363 | Eigen::Vector3f cls = dubinspath_.ps; 364 | cls(0) += R*(cosf(-M_PI_2_F)*cosf(dubinspath_.chis) - sinf(-M_PI_2_F)*sinf(dubinspath_.chis)); 365 | cls(1) += R*(sinf(-M_PI_2_F)*cosf(dubinspath_.chis) + cosf(-M_PI_2_F)*sinf(dubinspath_.chis)); 366 | Eigen::Vector3f cre = dubinspath_.pe; 367 | cre(0) += R*(cosf(M_PI_2_F)*cosf(dubinspath_.chie) - sinf(M_PI_2_F)*sinf(dubinspath_.chie)); 368 | cre(1) += R*(sinf(M_PI_2_F)*cosf(dubinspath_.chie) + cosf(M_PI_2_F)*sinf(dubinspath_.chie)); 369 | Eigen::Vector3f cle = dubinspath_.pe; 370 | cle(0) += R*(cosf(-M_PI_2_F)*cosf(dubinspath_.chie) - sinf(-M_PI_2_F)*sinf(dubinspath_.chie)); 371 | cle(1) += R*(sinf(-M_PI_2_F)*cosf(dubinspath_.chie) + cosf(-M_PI_2_F)*sinf(dubinspath_.chie)); 372 | 373 | float theta, theta2; 374 | // compute L1 375 | theta = atan2f(cre(1) - crs(1), cre(0) - crs(0)); 376 | 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)) 377 | + R*mo(2.0*M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta - M_PI_2_F)); 378 | 379 | // compute L2 380 | ell = (cle - crs).norm(); 381 | theta = atan2f(cle(1) - crs(1), cle(0) - crs(0)); 382 | float L2; 383 | if (2.0*R > ell) 384 | L2 = 9999.0f; 385 | else 386 | { 387 | theta2 = theta - M_PI_2_F + asinf(2.0*R/ell); 388 | L2 = sqrtf(ell*ell - 4.0*R*R) + R*mo(2.0*M_PI_F + mo(theta2) - mo(dubinspath_.chis - M_PI_2_F)) 389 | + R*mo(2.0*M_PI_F + mo(theta2 + M_PI_F) - mo(dubinspath_.chie + M_PI_2_F)); 390 | } 391 | 392 | // compute L3 393 | ell = (cre - cls).norm(); 394 | theta = atan2f(cre(1) - cls(1), cre(0) - cls(0)); 395 | float L3; 396 | if (2.0*R > ell) 397 | L3 = 9999.0f; 398 | else 399 | { 400 | theta2 = acosf(2.0*R/ell); 401 | L3 = sqrtf(ell*ell - 4*R*R) + R*mo(2.0*M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + theta2)) 402 | + R*mo(2.0*M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta + theta2 - M_PI_F)); 403 | } 404 | 405 | // compute L4 406 | theta = atan2f(cle(1) - cls(1), cle(0) - cls(0)); 407 | 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)) 408 | + R*mo(2.0*M_PI_F + mo(theta + M_PI_2_F) - mo(dubinspath_.chie + M_PI_2_F)); 409 | 410 | // L is the minimum distance 411 | int idx = 1; 412 | dubinspath_.L = L1; 413 | if (L2 < dubinspath_.L) 414 | { 415 | dubinspath_.L = L2; 416 | idx = 2; 417 | } 418 | if (L3 < dubinspath_.L) 419 | { 420 | dubinspath_.L = L3; 421 | idx = 3; 422 | } 423 | if (L4 < dubinspath_.L) 424 | { 425 | dubinspath_.L = L4; 426 | idx = 4; 427 | } 428 | 429 | Eigen::Vector3f e1; 430 | // e1.zero(); 431 | e1(0) = 1; 432 | e1(1) = 0; 433 | e1(2) = 0; 434 | switch (idx) 435 | { 436 | case 1: 437 | dubinspath_.cs = crs; 438 | dubinspath_.lams = 1; 439 | dubinspath_.ce = cre; 440 | dubinspath_.lame = 1; 441 | dubinspath_.q1 = (cre - crs).normalized(); 442 | dubinspath_.w1 = dubinspath_.cs + (rotz(-M_PI_2_F)*dubinspath_.q1)*R; 443 | dubinspath_.w2 = dubinspath_.ce + (rotz(-M_PI_2_F)*dubinspath_.q1)*R; 444 | break; 445 | case 2: 446 | dubinspath_.cs = crs; 447 | dubinspath_.lams = 1; 448 | dubinspath_.ce = cle; 449 | dubinspath_.lame = -1; 450 | ell = (cle - crs).norm(); 451 | theta = atan2f(cle(1) - crs(1), cle(0) - crs(0)); 452 | theta2 = theta - M_PI_2_F + asinf(2.0*R/ell); 453 | dubinspath_.q1 = rotz(theta2 + M_PI_2_F)*e1; 454 | dubinspath_.w1 = dubinspath_.cs + (rotz(theta2)*e1)*R; 455 | dubinspath_.w2 = dubinspath_.ce + (rotz(theta2 + M_PI_F)*e1)*R; 456 | break; 457 | case 3: 458 | dubinspath_.cs = cls; 459 | dubinspath_.lams = -1; 460 | dubinspath_.ce = cre; 461 | dubinspath_.lame = 1; 462 | ell = (cre - cls).norm(); 463 | theta = atan2f(cre(1) - cls(1), cre(0) - cls(0)); 464 | theta2 = acosf(2.0*R/ ell); 465 | dubinspath_.q1 = rotz(theta + theta2 - M_PI_2_F)*e1; 466 | dubinspath_.w1 = dubinspath_.cs + (rotz(theta + theta2)*e1)*R; 467 | dubinspath_.w2 = dubinspath_.ce + (rotz(theta + theta2 - M_PI_F)*e1)*R; 468 | break; 469 | case 4: 470 | dubinspath_.cs = cls; 471 | dubinspath_.lams = -1; 472 | dubinspath_.ce = cle; 473 | dubinspath_.lame = -1; 474 | dubinspath_.q1 = (cle - cls).normalized(); 475 | dubinspath_.w1 = dubinspath_.cs + (rotz(M_PI_2_F)*dubinspath_.q1)*R; 476 | dubinspath_.w2 = dubinspath_.ce + (rotz(M_PI_2_F)*dubinspath_.q1)*R; 477 | break; 478 | } 479 | dubinspath_.w3 = dubinspath_.pe; 480 | dubinspath_.q3 = rotz(dubinspath_.chie)*e1; 481 | dubinspath_.R = R; 482 | } 483 | } 484 | 485 | }//end namespace 486 | -------------------------------------------------------------------------------- /rosplane/src/path_planner.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define num_waypoints 3 5 | 6 | int main(int argc, char **argv) 7 | { 8 | ros::init(argc, argv, "rosplane_simple_path_planner"); 9 | 10 | ros::NodeHandle nh_; 11 | ros::Publisher waypointPublisher = nh_.advertise("waypoint_path", 10); 12 | 13 | float Va = 12; 14 | float wps[5*num_waypoints] = 15 | { 16 | 200, 0, -50, 45*M_PI/180, Va, 17 | 0, 200, -50, 45*M_PI/180, Va, 18 | 200, 200, -50, 225*M_PI/180, Va, 19 | }; 20 | 21 | for (int i(0); i < num_waypoints; i++) 22 | { 23 | ros::Duration(0.5).sleep(); 24 | 25 | rosplane_msgs::Waypoint new_waypoint; 26 | 27 | new_waypoint.w[0] = wps[i*5 + 0]; 28 | new_waypoint.w[1] = wps[i*5 + 1]; 29 | new_waypoint.w[2] = wps[i*5 + 2]; 30 | new_waypoint.chi_d = wps[i*5 + 3]; 31 | 32 | new_waypoint.chi_valid = true; 33 | new_waypoint.Va_d = wps[i*5 + 4]; 34 | if (i == 0) 35 | new_waypoint.set_current = true; 36 | else 37 | new_waypoint.set_current = false; 38 | new_waypoint.clear_wp_list = false; 39 | 40 | waypointPublisher.publish(new_waypoint); 41 | } 42 | ros::Duration(1.5).sleep(); 43 | 44 | return 0; 45 | } 46 | -------------------------------------------------------------------------------- /rosplane_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosplane_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | message_generation 7 | ) 8 | 9 | add_message_files( 10 | FILES 11 | Controller_Commands.msg 12 | Current_Path.msg 13 | Waypoint.msg 14 | Controller_Internals.msg 15 | State.msg 16 | ) 17 | 18 | generate_messages( 19 | DEPENDENCIES 20 | std_msgs 21 | ) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS std_msgs 25 | ) 26 | -------------------------------------------------------------------------------- /rosplane_msgs/msg/Controller_Commands.msg: -------------------------------------------------------------------------------- 1 | # Controller commands output from the path follower, input to autopilot controller 2 | 3 | # @warning Va_c, h_c and chi_c have always to be valid, the aux array is optional 4 | float32 Va_c # Commanded airspeed (m/s) 5 | float32 h_c # Commanded altitude (m) 6 | float32 chi_c # Commanded course (rad) 7 | float32 phi_ff # feed forward command for orbits (rad) 8 | float32[4] aux # Optional auxiliary commands 9 | bool aux_valid # Auxiliary commands valid 10 | -------------------------------------------------------------------------------- /rosplane_msgs/msg/Controller_Internals.msg: -------------------------------------------------------------------------------- 1 | # Controller internals (inner loops, states) output from the autopilot controller, for now just for debuging and ploting 2 | 3 | # @warning theta_c, phi_c and alt_zone have always to be valid, the aux array is optional 4 | float32 theta_c # Commanded pitch (rad) 5 | float32 phi_c # Commanded roll (rad) 6 | uint8 alt_zone # Zone in the altitude state machine 7 | float32[4] aux # Optional auxiliary commands 8 | bool aux_valid # Auxiliary commands valid 9 | 10 | uint8 ZONE_TAKE_OFF = 0 11 | uint8 ZONE_CLIMB = 1 12 | uint8 ZONE_DESEND = 2 13 | uint8 ZONE_ALTITUDE_HOLD = 3 14 | -------------------------------------------------------------------------------- /rosplane_msgs/msg/Current_Path.msg: -------------------------------------------------------------------------------- 1 | # Current path output from the path manager, input to path follower 2 | 3 | # @warning Va_d have always to be valid, 4 | # r and q need to be valid if path_type == LINE_PATH 5 | # c, rho, and, lambda need to be valid if path_type == ORBIT_PATH 6 | uint8 path_type # Indicates strait line or orbital path 7 | float32 Va_d # Desired airspeed (m/s) 8 | float32[3] r # Vector to origin of straight line path (m) 9 | float32[3] q # Unit vector, desired direction of travel for line path 10 | float32[3] c # Center of orbital path (m) 11 | float32 rho # Radius of orbital path (m) 12 | int8 lambda # Direction of orbital path (clockwise is 1, counterclockwise is -1) 13 | 14 | uint8 ORBIT_PATH = 0 15 | uint8 LINE_PATH = 1 16 | 17 | int8 CLOCKWISE = 1 18 | int8 COUNT_CLOCKWISE = -1 19 | -------------------------------------------------------------------------------- /rosplane_msgs/msg/State.msg: -------------------------------------------------------------------------------- 1 | # Vehicle state 'x_hat' output from the estimator or from simulator 2 | 3 | Header header 4 | 5 | # Original States 6 | # @warning roll, pitch and yaw have always to be valid, the quaternion is optional 7 | float32[3] position # north, east, down (m) 8 | float32 Va # Airspeed (m/s) 9 | float32 alpha # Angle of attack (rad) 10 | float32 beta # Slide slip angle (rad) 11 | float32 phi # Roll angle (rad) 12 | float32 theta # Pitch angle (rad) 13 | float32 psi # Yaw angle (rad) 14 | float32 chi # Course angle (rad) 15 | float32 p # Body frame rollrate (rad/s) 16 | float32 q # Body frame pitchrate (rad/s) 17 | float32 r # Body frame yawrate (rad/s) 18 | float32 Vg # Groundspeed (m/s) 19 | float32 wn # Windspeed north component (m/s) 20 | float32 we # Windspeed east component (m/s) 21 | 22 | # Additional States for convenience 23 | float32[4] quat # Quaternion (wxyz, NED) 24 | bool quat_valid # Quaternion valid 25 | float32 chi_deg # Wrapped course angle (deg) 26 | float32 psi_deg # Wrapped yaw angle (deg) 27 | float32 initial_lat # Initial/origin latitude (lat. deg) 28 | float32 initial_lon # Initial/origin longitude (lon. deg) 29 | float32 initial_alt # Initial/origin altitude (m) 30 | -------------------------------------------------------------------------------- /rosplane_msgs/msg/Waypoint.msg: -------------------------------------------------------------------------------- 1 | # New waypoint, input to path manager 2 | 3 | # @warning w and Va_d always have to be valid; the chi_d is optional. 4 | float32[3] w # Waypoint in local NED (m) 5 | float32 chi_d # Desired course at this waypoint (rad) 6 | bool chi_valid # Desired course valid (dubin or fillet paths) 7 | float32 Va_d # Desired airspeed (m/s) 8 | bool set_current # Sets this waypoint to be executed now! Starts a new list 9 | bool clear_wp_list # Removes all waypoints and returns to origin. The rest of 10 | # this message will be ignored 11 | -------------------------------------------------------------------------------- /rosplane_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosplane_msgs 4 | 0.0.0 5 | Message definitions for the ROSplane package 6 | 7 | Gary Ellingson 8 | 9 | Gary Ellingson 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | message_generation 16 | std_msgs 17 | geometry_msgs 18 | std_msgs 19 | geometry_msgs 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /rosplane_sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosplane_sim) 3 | 4 | find_package(gazebo) 5 | 6 | IF(gazebo_FOUND) 7 | 8 | set(CMAKE_CXX_STANDARD 11) 9 | if (NOT CMAKE_BUILD_TYPE) 10 | # Options: Debug, Release, MinSizeRel, RelWithDebInfo 11 | message(STATUS "No build type selected, default to Release") 12 | set(CMAKE_BUILD_TYPE "Release") 13 | endif() 14 | 15 | set(CMAKE_CXX_FLAGS "-fopenmp") 16 | 17 | # To enable assertions when compiled in release mode. 18 | add_definitions(-DROS_ASSERT_ENABLED) 19 | 20 | find_package(catkin REQUIRED COMPONENTS 21 | roscpp 22 | geometry_msgs 23 | rosflight_msgs 24 | rosplane_msgs 25 | gazebo_plugins 26 | gazebo_ros 27 | ) 28 | 29 | find_package(Eigen3 REQUIRED) 30 | link_directories(${GAZEBO_LIBRARY_DIRS}) 31 | 32 | catkin_package( 33 | INCLUDE_DIRS include 34 | CATKIN_DEPENDS roscpp gazebo_plugins gazebo_ros rosflight_msgs rosplane_msgs 35 | DEPENDS EIGEN3 GAZEBO 36 | ) 37 | 38 | include_directories( 39 | include 40 | ${catkin_INCLUDE_DIRS} 41 | ${EIGEN3_INCLUDE_DIRS} 42 | ${GAZEBO_INCLUDE_DIRS} 43 | ) 44 | 45 | add_library(aircraft_truth_plugin src/aircraft_truth.cpp) 46 | target_link_libraries(aircraft_truth_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 47 | add_dependencies(aircraft_truth_plugin ${catkin_EXPORTED_TARGETS}) 48 | 49 | add_library(aircraft_forces_and_moments_plugin src/aircraft_forces_and_moments.cpp) 50 | target_link_libraries(aircraft_forces_and_moments_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 51 | add_dependencies(aircraft_forces_and_moments_plugin ${catkin_EXPORTED_TARGETS}) 52 | 53 | ENDIF() 54 | -------------------------------------------------------------------------------- /rosplane_sim/include/rosplane_sim/aircraft_forces_and_moments.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 James Jackson, MAGICC Lab, Brigham Young University - Provo, UT 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef ROSPLANE_SIM_AIRCRAFT_FORCES_AND_MOMENTS_H 19 | #define ROSPLANE_SIM_AIRCRAFT_FORCES_AND_MOMENTS_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include "rosplane_sim/gz_compat.h" 38 | 39 | namespace gazebo 40 | { 41 | static const std::string kDefaultWindSpeedSubTopic = "gazebo/wind_speed"; 42 | 43 | 44 | class AircraftForcesAndMoments : public ModelPlugin 45 | { 46 | public: 47 | AircraftForcesAndMoments(); 48 | 49 | ~AircraftForcesAndMoments(); 50 | 51 | void InitializeParams(); 52 | void SendForces(); 53 | 54 | 55 | protected: 56 | void UpdateForcesAndMoments(); 57 | void Reset(); 58 | void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 59 | void OnUpdate(const common::UpdateInfo & /*_info*/); 60 | 61 | private: 62 | std::string command_topic_; 63 | std::string wind_speed_topic_; 64 | std::string joint_name_; 65 | std::string link_name_; 66 | std::string parent_frame_id_; 67 | std::string motor_speed_pub_topic_; 68 | std::string namespace_; 69 | 70 | physics::WorldPtr world_; 71 | physics::ModelPtr model_; 72 | physics::LinkPtr link_; 73 | physics::JointPtr joint_; 74 | physics::EntityPtr parent_link_; 75 | event::ConnectionPtr updateConnection_; // Pointer to the update event connection. 76 | 77 | // physical parameters 78 | double mass_; 79 | double Jx_; 80 | double Jy_; 81 | double Jz_; 82 | double Jxz_; 83 | double rho_; 84 | 85 | // aerodynamic coefficients 86 | struct WingCoeff 87 | { 88 | double S; 89 | double b; 90 | double c; 91 | double M; 92 | double epsilon; 93 | double alpha0; 94 | } wing_; 95 | 96 | // Propeller Coefficients 97 | struct PropCoeff 98 | { 99 | double k_motor; 100 | double k_T_P; 101 | double k_Omega; 102 | double e; 103 | double S; 104 | double C; 105 | } prop_; 106 | 107 | // Lift Coefficients 108 | struct LiftCoeff 109 | { 110 | double O; 111 | double alpha; 112 | double beta; 113 | double p; 114 | double q; 115 | double r; 116 | double delta_a; 117 | double delta_e; 118 | double delta_r; 119 | }; 120 | 121 | LiftCoeff CL_; 122 | LiftCoeff CD_; 123 | LiftCoeff Cm_; 124 | LiftCoeff CY_; 125 | LiftCoeff Cell_; 126 | LiftCoeff Cn_; 127 | 128 | // not constants 129 | // actuators 130 | struct Actuators 131 | { 132 | double e; 133 | double a; 134 | double r; 135 | double t; 136 | } delta_; 137 | 138 | // wind 139 | struct Wind 140 | { 141 | double N; 142 | double E; 143 | double D; 144 | } wind_; 145 | 146 | // container for forces 147 | struct ForcesAndTorques 148 | { 149 | double Fx; 150 | double Fy; 151 | double Fz; 152 | double l; 153 | double m; 154 | double n; 155 | } forces_; 156 | 157 | // Time Counters 158 | double sampling_time_ = 0; 159 | double prev_sim_time_ = 0; 160 | 161 | // For reset handling 162 | GazeboPose initial_pose_; 163 | 164 | ros::NodeHandle *nh_; 165 | ros::Subscriber command_sub_; 166 | ros::Subscriber wind_speed_sub_; 167 | 168 | boost::thread callback_queue_thread_; 169 | void WindSpeedCallback(const geometry_msgs::Vector3 &wind); 170 | void CommandCallback(const rosflight_msgs::CommandConstPtr &msg); 171 | 172 | GazeboVector wind_speed_W_; 173 | }; 174 | } 175 | 176 | #endif // ROSPLANE_SIM_AIRCRAFT_FORCES_AND_MOMENTS_H 177 | -------------------------------------------------------------------------------- /rosplane_sim/include/rosplane_sim/aircraft_truth.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Gary Ellingson, Brigham Young University, Provo UT 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | 18 | #ifndef ROSPLANE_SIM_AIRCRAFT_TRUTH_H 19 | #define ROSPLANE_SIM_AIRCRAFT_TRUTH_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include "rosplane_sim/common.h" 38 | #include "rosplane_sim/gz_compat.h" 39 | 40 | namespace gazebo 41 | { 42 | static const std::string kDefaultWindSpeedSubTopic = "gazebo/wind_speed"; 43 | 44 | 45 | class AircraftTruth : public ModelPlugin 46 | { 47 | public: 48 | AircraftTruth(); 49 | 50 | ~AircraftTruth(); 51 | 52 | void InitializeParams(); 53 | 54 | protected: 55 | void PublishTruth(); 56 | void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 57 | void OnUpdate(const common::UpdateInfo & /*_info*/); 58 | 59 | private: 60 | std::string truth_topic_; 61 | std::string wind_speed_topic_; 62 | std::string joint_name_; 63 | std::string link_name_; 64 | std::string parent_frame_id_; 65 | std::string motor_speed_pub_topic_; 66 | std::string namespace_; 67 | 68 | physics::WorldPtr world_; 69 | physics::ModelPtr model_; 70 | physics::LinkPtr link_; 71 | physics::JointPtr joint_; 72 | physics::EntityPtr parent_link_; 73 | event::ConnectionPtr updateConnection_; // Pointer to the update event connection. 74 | 75 | // wind 76 | struct Wind 77 | { 78 | double N; 79 | double E; 80 | double D; 81 | } wind_; 82 | 83 | // Time Counters 84 | double sampling_time_; 85 | double prev_sim_time_; 86 | 87 | ros::NodeHandle *nh_; 88 | ros::Subscriber wind_speed_sub_; 89 | ros::Publisher true_state_pub_; 90 | 91 | boost::thread callback_queue_thread_; 92 | void QueueThread(); 93 | void WindSpeedCallback(const geometry_msgs::Vector3 &wind); 94 | 95 | std::unique_ptr> rotor_velocity_filter_; 96 | GazeboVector wind_speed_W_; 97 | }; 98 | } 99 | 100 | #endif // ROSPLANE_SIM_AIRCRAFT_TRUTH_H 101 | -------------------------------------------------------------------------------- /rosplane_sim/include/rosplane_sim/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland 5 | * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland 6 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | #ifndef ROSPLANE_SIM_COMMON_H_ 22 | #define ROSPLANE_SIM_COMMON_H_ 23 | 24 | #include 25 | #include 26 | 27 | namespace gazebo 28 | { 29 | 30 | // Default values 31 | static const std::string kDefaultNamespace = ""; 32 | static constexpr double kDefaultRotorVelocitySlowdownSim = 10.0; 33 | 34 | /** 35 | * \brief Obtains a parameter from sdf. 36 | * \param[in] sdf Pointer to the sdf object. 37 | * \param[in] name Name of the parameter. 38 | * \param[out] param Param Variable to write the parameter to. 39 | * \param[in] default_value Default value, if the parameter not available. 40 | * \param[in] verbose If true, gzerror if the parameter is not available. 41 | */ 42 | template 43 | bool getSdfParam(sdf::ElementPtr sdf, const std::string &name, T ¶m, const T &default_value, const bool &verbose = 44 | false) 45 | { 46 | if (sdf->HasElement(name)) 47 | { 48 | param = sdf->GetElement(name)->Get(); 49 | return true; 50 | } 51 | else 52 | { 53 | param = default_value; 54 | if (verbose) 55 | gzerr << "[rosplane_sim] Please specify a value for parameter \"" << name << "\".\n"; 56 | } 57 | return false; 58 | } 59 | 60 | } 61 | 62 | template 63 | class FirstOrderFilter 64 | { 65 | /* 66 | This class can be used to apply a first order filter on a signal. 67 | It allows different acceleration and deceleration time constants. 68 | 69 | Short reveiw of discrete time implementation of firest order system: 70 | Laplace: 71 | X(s)/U(s) = 1/(tau*s + 1) 72 | continous time system: 73 | dx(t) = (-1/tau)*x(t) + (1/tau)*u(t) 74 | discretized system (ZoH): 75 | x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau)))*u(k) 76 | */ 77 | 78 | public: 79 | FirstOrderFilter(double timeConstantUp, double timeConstantDown, T initialState): 80 | timeConstantUp_(timeConstantUp), 81 | timeConstantDown_(timeConstantDown), 82 | previousState_(initialState) {} 83 | 84 | T updateFilter(T inputState, double samplingTime) 85 | { 86 | /* 87 | This method will apply a first order filter on the inputState. 88 | */ 89 | T outputState; 90 | if (inputState > previousState_) 91 | { 92 | // Calcuate the outputState if accelerating. 93 | double alphaUp = exp(- samplingTime/timeConstantUp_); 94 | // x(k+1) = Ad*x(k) + Bd*u(k) 95 | outputState = alphaUp*previousState_ + (1 - alphaUp)*inputState; 96 | 97 | } 98 | else 99 | { 100 | // Calculate the outputState if decelerating. 101 | double alphaDown = exp(- samplingTime/timeConstantDown_); 102 | outputState = alphaDown*previousState_ + (1 - alphaDown)*inputState; 103 | } 104 | previousState_ = outputState; 105 | return outputState; 106 | 107 | } 108 | ~FirstOrderFilter() {} 109 | 110 | protected: 111 | double timeConstantUp_; 112 | double timeConstantDown_; 113 | T previousState_; 114 | }; 115 | 116 | 117 | 118 | /// Computes a quaternion from the 3-element small angle approximation theta. 119 | template 120 | Eigen::Quaternion QuaternionFromSmallAngle(const Eigen::MatrixBase &theta) 121 | { 122 | typedef typename Derived::Scalar Scalar; 123 | EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); 124 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); 125 | const Scalar q_squared = theta.squaredNorm()/4.0; 126 | 127 | if (q_squared < 1) 128 | { 129 | return Eigen::Quaternion(sqrt(1 - q_squared), theta[0]*0.5, theta[1]*0.5, theta[2]*0.5); 130 | } 131 | else 132 | { 133 | const Scalar w = 1.0/sqrt(1 + q_squared); 134 | const Scalar f = w*0.5; 135 | return Eigen::Quaternion(w, theta[0]*f, theta[1]*f, theta[2]*f); 136 | } 137 | } 138 | 139 | template 140 | void copyPosition(const In &in, Out *out) 141 | { 142 | out->x = in.x; 143 | out->y = in.y; 144 | out->z = in.z; 145 | } 146 | 147 | #endif /* ROSPLANE_SIM_COMMON_H_ */ 148 | -------------------------------------------------------------------------------- /rosplane_sim/include/rosplane_sim/gz_compat.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2018 Trey Henrichsen, Daniel Koch, James Jackson and Gary Ellingson, BYU MAGICC Lab. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 11 | * * Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 15 | * * Neither the name of the copyright holder nor the names of its 16 | * contributors may be used to endorse or promote products derived from 17 | * this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 23 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | * POSSIBILITY OF SUCH DAMAGE. 30 | */ 31 | 32 | #ifndef ROSPLANE_SIM_GZ_COMPAT_H //Include guard 33 | #define ROSPLANE_SIM_GZ_COMPAT_H 34 | 35 | #include 36 | 37 | #if GAZEBO_MAJOR_VERSION >= 8 38 | 39 | using GazeboVector = ignition::math::Vector3d; 40 | using GazeboPose = ignition::math::Pose3d; 41 | using GazeboQuaternion = ignition::math::Quaterniond; 42 | 43 | #define GZ_COMPAT_GET_X(VECTOR) (VECTOR).X() 44 | #define GZ_COMPAT_GET_Y(VECTOR) (VECTOR).Y() 45 | #define GZ_COMPAT_GET_Z(VECTOR) (VECTOR).Z() 46 | #define GZ_COMPAT_GET_W(VECTOR) (VECTOR).W() //For quaternions 47 | #define GZ_COMPAT_SET_X(VECTOR,VALUE) (VECTOR).X((VALUE)) 48 | #define GZ_COMPAT_SET_Y(VECTOR,VALUE) (VECTOR).Y((VALUE)) 49 | #define GZ_COMPAT_SET_Z(VECTOR,VALUE) (VECTOR).Z((VALUE)) 50 | #define GZ_COMPAT_SET_W(VECTOR,VALUE) (VECTOR).W((VALUE)) 51 | #define GZ_COMPAT_GET_POS(POSE) (POSE).Pos() 52 | #define GZ_COMPAT_GET_ROT(POSE) (POSE).Rot() 53 | #define GZ_COMPAT_GET_EULER(QUAT) (QUAT).Euler() 54 | #define GZ_COMPAT_GET_SIM_TIME(WORLD_PTR) (WORLD_PTR)->SimTime() 55 | #define GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(LINK_PTR) (LINK_PTR)->RelativeLinearVel() 56 | #define GZ_COMPAT_GET_RELATIVE_ANGULAR_VEL(LINK_PTR) (LINK_PTR)->RelativeAngularVel() 57 | #define GZ_COMPAT_GET_WORLD_COG_POSE(LINK_PTR) (LINK_PTR)->WorldCoGPose() 58 | #define GZ_COMPAT_GET_WORLD_POSE(LINK_PTR) (LINK_PTR)->WorldPose() 59 | #define GZ_COMPAT_GET_RELATIVE_FORCE(LINK_PTR) (LINK_PTR)->RelativeForce() 60 | #define GZ_COMPAT_GET_ENTITY(WORLD_PTR,FRAME_ID_PTR) (WORLD_PTR)->EntityByName((FRAME_ID_PTR)) 61 | #define GZ_COMPAT_GET_WORLD_LINEAR_ACCEL(LINK_PTR) (LINK_PTR)->WorldLinearAccel() 62 | #define GZ_COMPAT_GET_LENGTH(VECTOR) (VECTOR).Length() 63 | #define GZ_COMPAT_DISCONNECT_WORLD_UPDATE_BEGIN(CONNECTION) (CONNECTION).reset() 64 | #define GZ_COMPAT_GET_GRAVITY(WORLD_PTR) (WORLD_PTR)->Gravity() 65 | #define GZ_COMPAT_GET_MASS(INERTIAL_PTR) (INERTIAL_PTR)->Mass() 66 | 67 | #else //I.E. GAZEBO_MAJOR_VERSION < 8 68 | 69 | using GazeboVector = gazebo::math::Vector3; 70 | using GazeboPose = gazebo::math::Pose; 71 | using GazeboQuaternion = gazebo::math::Quaternion; 72 | 73 | #define GZ_COMPAT_GET_X(VECTOR) (VECTOR).x 74 | #define GZ_COMPAT_GET_Y(VECTOR) (VECTOR).y 75 | #define GZ_COMPAT_GET_Z(VECTOR) (VECTOR).z 76 | #define GZ_COMPAT_GET_W(VECTOR) (VECTOR).w //For quaternions 77 | #define GZ_COMPAT_SET_X(VECTOR,VALUE) (VECTOR).x = (VALUE) 78 | #define GZ_COMPAT_SET_Y(VECTOR,VALUE) (VECTOR).y = (VALUE) 79 | #define GZ_COMPAT_SET_Z(VECTOR,VALUE) (VECTOR).z = (VALUE) 80 | #define GZ_COMPAT_SET_W(VECTOR,VALUE) (VECTOR).w = (VALUE) 81 | #define GZ_COMPAT_GET_POS(POSE) (POSE).pos 82 | #define GZ_COMPAT_GET_ROT(POSE) (POSE).rot 83 | #define GZ_COMPAT_GET_EULER(QUAT) (QUAT).GetAsEuler() 84 | #define GZ_COMPAT_GET_SIM_TIME(WORLD_PTR) (WORLD_PTR)->GetSimTime() 85 | #define GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(LINK_PTR) (LINK_PTR)->GetRelativeLinearVel() 86 | #define GZ_COMPAT_GET_RELATIVE_ANGULAR_VEL(LINK_PTR) (LINK_PTR)->GetRelativeAngularVel() 87 | #define GZ_COMPAT_GET_WORLD_COG_POSE(LINK_PTR) (LINK_PTR)->GetWorldCoGPose() 88 | #define GZ_COMPAT_GET_WORLD_POSE(LINK_PTR) (LINK_PTR)->GetWorldPose() 89 | #define GZ_COMPAT_GET_RELATIVE_FORCE(LINK_PTR) (LINK_PTR)->GetRelativeForce() 90 | #define GZ_COMPAT_GET_ENTITY(WORLD_PTR,FRAME_ID_PTR) (WORLD_PTR)->GetEntity((FRAME_ID_PTR)) 91 | #define GZ_COMPAT_GET_WORLD_LINEAR_ACCEL(LINK_PTR) (LINK_PTR)->GetWorldLinearAccel() 92 | #define GZ_COMPAT_GET_LENGTH(VECTOR) (VECTOR).GetLength() 93 | #define GZ_COMPAT_DISCONNECT_WORLD_UPDATE_BEGIN(CONNECTION) gazebo::event::Events::DisconnectWorldUpdateBegin((CONNECTION)) 94 | #define GZ_COMPAT_GET_GRAVITY(WORLD_PTR) (WORLD_PTR)->GetPhysicsEngine()->GetGravity() 95 | #define GZ_COMPAT_GET_MASS(INERTIAL_PTR) (INERTIAL_PTR)->GetMass() 96 | 97 | #endif //GAZEBO_MAJOR_VERSION >= 8 98 | 99 | #endif //Include guard 100 | -------------------------------------------------------------------------------- /rosplane_sim/launch/fixedwing.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 34 | 35 | 36 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /rosplane_sim/launch/rosflight_sil.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /rosplane_sim/launch/spawn_mav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 25 | 26 | 27 | 28 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /rosplane_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosplane_sim 4 | 0.1.3 5 | Simulator tools for the ROSplane package 6 | 7 | Gary Ellingson 8 | 9 | Gary Ellingson 10 | James Jackson 11 | Craig Bidstrup 12 | 13 | BSD 14 | 15 | catkin 16 | 17 | gazebo_plugins 18 | gazebo_ros 19 | geometry_msgs 20 | roscpp 21 | rosflight_msgs 22 | rosplane_msgs 23 | rosflight_plugins 24 | 25 | eigen 26 | gazebo 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /rosplane_sim/params/fixedwing.yaml: -------------------------------------------------------------------------------- 1 | # Based on the My Twin Dream airframe (1.8m wingspan) 2 | mass: 3.92 3 | 4 | Jx: 0.213 5 | Jy: 0.171 6 | Jz: 0.350 7 | Jxz: 0.04 8 | 9 | rho: 1.2682 10 | wing_s: 0.468 11 | wing_b: 1.8 12 | wing_c: 0.26 13 | wing_M: 50 14 | wing_epsilon: 0.1592 15 | wing_alpha0: 0.3040363557 16 | 17 | k_motor: 30.0 18 | k_T_P: 0.0 19 | k_Omega: 0.0 20 | 21 | prop_e: 0.8 22 | prop_S: 0.0314 23 | prop_C: 1.0 24 | 25 | C_L_O: 0.2869 26 | C_L_alpha: 5.1378 27 | C_L_beta: 0.0 28 | C_L_p: 0.0 29 | C_L_q: 1.7102 30 | C_L_r: 0.0 31 | C_L_delta_a: 0.0 32 | C_L_delta_e: 0.5202 33 | C_L_delta_r: 0.0 34 | 35 | C_D_O: 0.03087 36 | C_D_alpha: 0.0043021 37 | C_D_beta: 0.0 38 | C_D_p: 0.02815 39 | C_D_q: 0.2514 40 | C_D_r: 0.0 41 | C_D_delta_a: 0.0 42 | C_D_delta_e: 0.01879 43 | C_D_delta_r: 0.0 44 | 45 | C_ell_O: 0.0 46 | C_ell_alpha: 0.00 47 | C_ell_beta: 0.0193 48 | C_ell_p: -0.5406 49 | C_ell_q: 0.0 50 | C_ell_r: 0.1929 51 | C_ell_delta_a: 0.2818 52 | C_ell_delta_e: 0.0 53 | C_ell_delta_r: 0.00096 54 | 55 | C_m_O: 0.0362 56 | C_m_alpha: -0.2627 57 | C_m_beta: 0.0 58 | C_m_p: 0.0 59 | C_m_q: -9.7213 60 | C_m_r: 0.0 61 | C_m_delta_a: 0.0 62 | C_m_delta_e: -1.2392 63 | C_m_delta_r: 0.0 64 | 65 | C_n_O: 0.0 66 | C_n_alpha: 0.0 67 | C_n_beta: 0.08557 68 | C_n_p: -0.0498 69 | C_n_q: 0.0 70 | C_n_r: -0.0572 71 | C_n_delta_a: 0.0095 72 | C_n_delta_e: 0.0 73 | C_n_delta_r: -0.06 74 | 75 | C_Y_O: 0.0 76 | C_Y_alpha: 0.00 77 | C_Y_beta: -0.2471 78 | C_Y_p: -0.07278 79 | C_Y_q: 0.0 80 | C_Y_r: 0.1849 81 | C_Y_delta_a: -0.02344 82 | C_Y_delta_e: 0.0 83 | C_Y_delta_r: 0.1591 84 | 85 | # trim conditions 86 | phi0: 0 87 | theta0: 0.0349 # 2 degrees 88 | psi0: 0 89 | Va0: 16.38 90 | delta_e0: 0.02967 # 1.7 degrees 91 | delta_t0: 0.4 92 | 93 | # ============================= 94 | # ======= Plugin Params ======= 95 | # ============================= 96 | 97 | # Truth 98 | wind_speed_topic: "gazebo/wind_speed" 99 | truthTopic: "truth" 100 | 101 | # Forces and Moments 102 | windSpeedTopic: "wind" 103 | commandTopic: "command" 104 | 105 | # Sensor Noise Parameters (These are empirically-determined) 106 | gyro_stdev: 0.02 107 | gyro_bias_range: 0.25 108 | gyro_bias_walk_stdev: 0.00001 109 | 110 | acc_stdev: 0.19 111 | acc_bias_range: 0.6 112 | acc_bias_walk_stdev: 0.00001 113 | 114 | baro_stdev: 4.0 115 | baro_bias_range: 500 116 | baro_bias_walk_stdev: 0.1 117 | 118 | sonar_stdev: 0.03 119 | sonar_min_range: 0.25 120 | sonar_max_range: 8.0 121 | 122 | # TODO: update these with empirically-derived values 123 | airspeed_stdev: 1.15 124 | airspeed_bias_range: 0.15 125 | airspeed_bias_walk_stdev: 0.001 126 | -------------------------------------------------------------------------------- /rosplane_sim/src/aircraft_forces_and_moments.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 James Jackson, MAGICC Lab, Brigham Young University, Provo, UT 3 | * Copyright 2016 Gary Ellingson, MAGICC Lab, Brigham Young University, Provo, UT 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include "rosplane_sim/aircraft_forces_and_moments.h" 19 | 20 | namespace gazebo 21 | { 22 | 23 | AircraftForcesAndMoments::AircraftForcesAndMoments() {} 24 | 25 | 26 | AircraftForcesAndMoments::~AircraftForcesAndMoments() 27 | { 28 | GZ_COMPAT_DISCONNECT_WORLD_UPDATE_BEGIN(updateConnection_); 29 | if (nh_) 30 | { 31 | nh_->shutdown(); 32 | delete nh_; 33 | } 34 | } 35 | 36 | void AircraftForcesAndMoments::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 37 | { 38 | model_ = _model; 39 | world_ = model_->GetWorld(); 40 | namespace_.clear(); 41 | 42 | /* 43 | * Connect the Plugin to the Robot and Save pointers to the various elements in the simulation 44 | */ 45 | if (_sdf->HasElement("namespace")) 46 | namespace_ = _sdf->GetElement("namespace")->Get(); 47 | else 48 | gzerr << "[gazebo_aircraft_forces_and_moments] Please specify a namespace.\n"; 49 | nh_ = new ros::NodeHandle(namespace_); 50 | 51 | if (_sdf->HasElement("linkName")) 52 | link_name_ = _sdf->GetElement("linkName")->Get(); 53 | else 54 | gzerr << "[gazebo_aircraft_forces_and_moments] Please specify a linkName of the forces and moments plugin.\n"; 55 | link_ = model_->GetLink(link_name_); 56 | if (link_ == NULL) 57 | gzthrow("[gazebo_aircraft_forces_and_moments] Couldn't find specified link \"" << link_name_ << "\"."); 58 | 59 | /* Load Params from Gazebo Server */ 60 | wind_speed_topic_ = nh_->param("windSpeedTopic", "wind"); 61 | command_topic_ = nh_->param("commandTopic", "command"); 62 | 63 | // The following parameters are aircraft-specific, most of these can be found using AVL 64 | // The rest are more geometry-based and can be found in conventional methods 65 | // For the moments of inertia, look into using the BiFilar pendulum method 66 | 67 | // physical parameters 68 | mass_ = nh_->param("mass", 3.92); 69 | Jx_ = nh_->param("Jx", 0.213); 70 | Jy_ = nh_->param("Jy", 0.171); 71 | Jz_ = nh_->param("Jz", 0.350); 72 | Jxz_ = nh_->param("Jxz", 0.04); 73 | rho_ = nh_->param("rho", 1.268); 74 | 75 | // Wing Geometry 76 | wing_.S = nh_->param("wing_s", 0.468); 77 | wing_.b = nh_->param("wing_b", 1.8); 78 | wing_.c = nh_->param("wing_c", 0.26); 79 | wing_.M = nh_->param("wing_M", 50); 80 | wing_.epsilon = nh_->param("wing_epsilon", 0.159); 81 | wing_.alpha0 = nh_->param("wing_alpha0", 0.304); 82 | 83 | // Propeller Coefficients 84 | prop_.k_motor = nh_->param("k_motor", 40.0); 85 | prop_.k_T_P = nh_->param("k_T_P", 0.0); 86 | prop_.k_Omega = nh_->param("k_Omega", 0.0); 87 | prop_.e = nh_->param("prop_e", 0.8); 88 | prop_.S = nh_->param("prop_S", 0.0314); 89 | prop_.C = nh_->param("prop_C", 1.0); 90 | 91 | // Lift Params 92 | CL_.O = nh_->param("C_L_O", 0.2869); 93 | CL_.alpha = nh_->param("C_L_alpha", 5.1378); 94 | CL_.beta = nh_->param("C_L_beta", 0.0); 95 | CL_.p = nh_->param("C_L_p", 0.0); 96 | CL_.q = nh_->param("C_L_q", 1.7102); 97 | CL_.r = nh_->param("C_L_r", 0.0); 98 | CL_.delta_a = nh_->param("C_L_delta_a", 0.0); 99 | CL_.delta_e = nh_->param("C_L_delta_e", 0.5202); 100 | CL_.delta_r = nh_->param("C_L_delta_r", 0.0); 101 | 102 | // Drag Params 103 | CD_.O = nh_->param("C_D_O", 0.03087); 104 | CD_.alpha = nh_->param("C_D_alpha", 0.0043021); 105 | CD_.beta = nh_->param("C_D_beta", 0.0); 106 | CD_.p = nh_->param("C_D_p", 0.02815); 107 | CD_.q = nh_->param("C_D_q", 0.2514); 108 | CD_.r = nh_->param("C_D_r", 0.0); 109 | CD_.delta_a = nh_->param("C_D_delta_a", 0.0); 110 | CD_.delta_e = nh_->param("C_D_delta_e", 0.01879); 111 | CD_.delta_r = nh_->param("C_D_delta_r", 0.0); 112 | 113 | // ell Params (x axis moment) 114 | Cell_.O = nh_->param("C_ell_O", 0.0); 115 | Cell_.alpha = nh_->param("C_ell_alpha", 0.00); 116 | Cell_.beta = nh_->param("C_ell_beta", 0.0193); 117 | Cell_.p = nh_->param("C_ell_p", -0.5406); 118 | Cell_.q = nh_->param("C_ell_q", 0.0); 119 | Cell_.r = nh_->param("C_ell_r", 0.1929); 120 | Cell_.delta_a = nh_->param("C_ell_delta_a", 0.2818); 121 | Cell_.delta_e = nh_->param("C_ell_delta_e", 0.0); 122 | Cell_.delta_r = nh_->param("C_ell_delta_r", 0.00096); 123 | 124 | // m Params (y axis moment) 125 | Cm_.O = nh_->param("C_m_O", 0.0362); 126 | Cm_.alpha = nh_->param("C_m_alpha", -0.2627); 127 | Cm_.beta = nh_->param("C_m_beta", 0.0); 128 | Cm_.p = nh_->param("C_m_p", 0.0); 129 | Cm_.q = nh_->param("C_m_q", -9.7213); 130 | Cm_.r = nh_->param("C_m_r", 0.0); 131 | Cm_.delta_a = nh_->param("C_m_delta_a", 0.0); 132 | Cm_.delta_e = nh_->param("C_m_delta_e", -1.2392); 133 | Cm_.delta_r = nh_->param("C_m_delta_r", 0.0); 134 | 135 | // n Params (z axis moment) 136 | Cn_.O = nh_->param("C_n_O", 0.0); 137 | Cn_.alpha = nh_->param("C_n_alpha", 0.0); 138 | Cn_.beta = nh_->param("C_n_beta", 0.08557); 139 | Cn_.p = nh_->param("C_n_p", -0.0498); 140 | Cn_.q = nh_->param("C_n_q", 0.0); 141 | Cn_.r = nh_->param("C_n_r", -0.0572); 142 | Cn_.delta_a = nh_->param("C_n_delta_a", 0.0095); 143 | Cn_.delta_e = nh_->param("C_n_delta_e", 0.0); 144 | Cn_.delta_r = nh_->param("C_n_delta_r", -0.06); 145 | 146 | // Y Params (Sideslip Forces) 147 | CY_.O = nh_->param("C_Y_O", 0.0); 148 | CY_.alpha = nh_->param("C_Y_alpha", 0.00); 149 | CY_.beta = nh_->param("C_Y_beta", -0.2471); 150 | CY_.p = nh_->param("C_Y_p", -0.07278); 151 | CY_.q = nh_->param("C_Y_q", 0.0); 152 | CY_.r = nh_->param("C_Y_r", 0.1849); 153 | CY_.delta_a = nh_->param("C_Y_delta_a", -0.02344); 154 | CY_.delta_e = nh_->param("C_Y_delta_e", 0.0); 155 | CY_.delta_r = nh_->param("C_Y_delta_r", 0.1591); 156 | 157 | // Initialize Wind 158 | wind_.N = 0.0; 159 | wind_.E = 0.0; 160 | wind_.D = 0.0; 161 | 162 | //initialize deltas 163 | delta_.t = 0.0; 164 | delta_.e = 0.0; 165 | delta_.a = 0.0; 166 | delta_.r = 0.0; 167 | 168 | // Connect the update function to the simulation 169 | updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&AircraftForcesAndMoments::OnUpdate, this, _1)); 170 | 171 | // Connect Subscribers 172 | command_sub_ = nh_->subscribe(command_topic_, 1, &AircraftForcesAndMoments::CommandCallback, this); 173 | wind_speed_sub_ = nh_->subscribe(wind_speed_topic_, 1, &AircraftForcesAndMoments::WindSpeedCallback, this); 174 | 175 | // Pull off initial pose so we can reset to it 176 | initial_pose_ = GZ_COMPAT_GET_WORLD_COG_POSE(link_); 177 | } 178 | 179 | // This gets called by the world update event. 180 | void AircraftForcesAndMoments::OnUpdate(const common::UpdateInfo &_info) 181 | { 182 | sampling_time_ = _info.simTime.Double() - prev_sim_time_; 183 | prev_sim_time_ = _info.simTime.Double(); 184 | UpdateForcesAndMoments(); 185 | SendForces(); 186 | } 187 | 188 | void AircraftForcesAndMoments::Reset() 189 | { 190 | forces_.Fx = 0.0; 191 | forces_.Fy = 0.0; 192 | forces_.Fz = 0.0; 193 | forces_.l = 0.0; 194 | forces_.m = 0.0; 195 | forces_.n = 0.0; 196 | 197 | link_->SetWorldPose(initial_pose_); 198 | link_->ResetPhysicsStates(); 199 | } 200 | 201 | void AircraftForcesAndMoments::WindSpeedCallback(const geometry_msgs::Vector3 &wind) 202 | { 203 | wind_.N = wind.x; 204 | wind_.E = wind.y; 205 | wind_.D = wind.z; 206 | } 207 | 208 | void AircraftForcesAndMoments::CommandCallback(const rosflight_msgs::CommandConstPtr &msg) 209 | { 210 | // This is a little bit weird. We need to nail down why these are negative 211 | delta_.t = msg->F; 212 | delta_.e = -msg->y; 213 | delta_.a = msg->x; 214 | delta_.r = -msg->z; 215 | } 216 | 217 | 218 | void AircraftForcesAndMoments::UpdateForcesAndMoments() 219 | { 220 | /* Get state information from Gazebo (in NED) * 221 | * C denotes child frame, P parent frame, and W world frame. * 222 | // * Further C_pose_W_P denotes pose of P wrt. W expressed in C.*/ 223 | GazeboVector C_linear_velocity_W_C = GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(link_); 224 | double u = GZ_COMPAT_GET_X(C_linear_velocity_W_C); 225 | double v = -GZ_COMPAT_GET_Y(C_linear_velocity_W_C); 226 | double w = -GZ_COMPAT_GET_Z(C_linear_velocity_W_C); 227 | GazeboVector C_angular_velocity_W_C = GZ_COMPAT_GET_RELATIVE_ANGULAR_VEL(link_); 228 | double p = GZ_COMPAT_GET_X(C_angular_velocity_W_C); 229 | double q = -GZ_COMPAT_GET_Y(C_angular_velocity_W_C); 230 | double r = -GZ_COMPAT_GET_Z(C_angular_velocity_W_C); 231 | 232 | // wind info is available in the wind_ struct 233 | /// TODO: This is wrong. Wind is being applied in the body frame, not inertial frame 234 | double ur = u - wind_.N; 235 | double vr = v - wind_.E; 236 | double wr = w - wind_.D; 237 | 238 | double Va = sqrt(pow(ur, 2.0) + pow(vr, 2.0) + pow(wr, 2.0)); 239 | 240 | // Don't divide by zero, and don't let NaN's get through (sometimes GetRelativeLinearVel returns NaNs) 241 | if (Va > 0.000001 && std::isfinite(Va)) 242 | { 243 | /* 244 | * The following math follows the method described in chapter 4 of 245 | * Small Unmanned Aircraft: Theory and Practice 246 | * By Randy Beard and Tim McLain. 247 | * Look there for a detailed explanation of each line in the rest of this function 248 | */ 249 | double alpha = atan2(wr , ur); 250 | double beta = asin(vr/Va); 251 | 252 | double sign = (alpha >= 0 ? 1 : -1); //Sigmoid function 253 | double sigma_a = (1 + exp(-(wing_.M*(alpha - wing_.alpha0))) + exp((wing_.M*(alpha + wing_.alpha0))))/((1 + exp(- 254 | (wing_.M*(alpha - wing_.alpha0))))*(1 + exp((wing_.M*(alpha + wing_.alpha0))))); 255 | double CL_a = (1 - sigma_a)*(CL_.O + CL_.alpha*alpha) + sigma_a*(2.0*sign*pow(sin(alpha), 2.0)*cos(alpha)); 256 | double AR = (pow(wing_.b, 2.0))/wing_.S; 257 | double CD_a = CD_.p + ((pow((CL_.O + CL_.alpha*(alpha)), 258 | 2.0))/(3.14159*0.9 * 259 | AR)); //the const 0.9 in this equation replaces the e (Oswald Factor) variable and may be inaccurate 260 | 261 | double CX_a = -CD_a*cos(alpha) + CL_a*sin(alpha); 262 | double CX_q_a = -CD_.q*cos(alpha) + CL_.q*sin(alpha); 263 | double CX_deltaE_a = -CD_.delta_e*cos(alpha) + CL_.delta_e*sin(alpha); 264 | 265 | double CZ_a = -CD_a*sin(alpha) - CL_a*cos(alpha); 266 | double CZ_q_a = -CD_.q*sin(alpha) - CL_.q*cos(alpha); 267 | double CZ_deltaE_a = -CD_.delta_e*sin(alpha) - CL_.delta_e*cos(alpha); 268 | 269 | forces_.Fx = 0.5*(rho_)*pow(Va, 2.0)*wing_.S*(CX_a + (CX_q_a*wing_.c*q) / 270 | (2.0*Va) + CX_deltaE_a*delta_.e) + 0.5*rho_*prop_.S*prop_.C*(pow((prop_.k_motor*delta_.t), 2.0) - pow(Va, 271 | 2.0)); 272 | forces_.Fy = 0.5*(rho_)*pow(Va, 2.0)*wing_.S*(CY_.O + CY_.beta*beta + ((CY_.p*wing_.b*p) / 273 | (2.0*Va)) + ((CY_.r*wing_.b*r)/(2.0*Va)) + CY_.delta_a*delta_.a + CY_.delta_r*delta_.r); 274 | forces_.Fz = 0.5*(rho_)*pow(Va, 2.0)*wing_.S*(CZ_a + (CZ_q_a*wing_.c*q) / 275 | (2.0*Va) + CZ_deltaE_a*delta_.e); 276 | 277 | forces_.l = 0.5*(rho_)*pow(Va, 2.0)*wing_.S*wing_.b*(Cell_.O + Cell_.beta*beta + (Cell_.p*wing_.b*p) / 278 | (2.0*Va) + (Cell_.r*wing_.b*r)/(2.0*Va) + Cell_.delta_a*delta_.a + Cell_.delta_r*delta_.r) - prop_.k_T_P * 279 | pow((prop_.k_Omega*delta_.t), 2.0); 280 | forces_.m = 0.5*(rho_)*pow(Va, 2.0)*wing_.S*wing_.c*(Cm_.O + Cm_.alpha*alpha + (Cm_.q*wing_.c*q) / 281 | (2.0*Va) + Cm_.delta_e*delta_.e); 282 | forces_.n = 0.5*(rho_)*pow(Va, 2.0)*wing_.S*wing_.b*(Cn_.O + Cn_.beta*beta + (Cn_.p*wing_.b*p) / 283 | (2.0*Va) + (Cn_.r*wing_.b*r)/(2.0*Va) + Cn_.delta_a*delta_.a + Cn_.delta_r*delta_.r); 284 | } 285 | else 286 | { 287 | if (!std::isfinite(Va)) 288 | { 289 | gzerr << "u = " << u << "\n"; 290 | gzerr << "v = " << v << "\n"; 291 | gzerr << "w = " << w << "\n"; 292 | gzerr << "p = " << p << "\n"; 293 | gzerr << "q = " << q << "\n"; 294 | gzerr << "r = " << r << "\n"; 295 | gzerr << "ur = " << ur << "\n"; 296 | gzerr << "vr = " << vr << "\n"; 297 | gzerr << "wr = " << wr << "\n"; 298 | gzthrow("we have a NaN or an infinity:\n"); 299 | } 300 | else 301 | { 302 | forces_.Fx = 0.5*rho_*prop_.S*prop_.C*(pow((prop_.k_motor*delta_.t), 2.0)); 303 | forces_.Fy = 0.0; 304 | forces_.Fz = 0.0; 305 | forces_.l = 0.0; 306 | forces_.m = 0.0; 307 | forces_.n = 0.0; 308 | } 309 | } 310 | } 311 | 312 | 313 | void AircraftForcesAndMoments::SendForces() 314 | { 315 | // Make sure we are applying reasonable forces 316 | if (std::isfinite(forces_.Fx + forces_.Fy + forces_.Fz + forces_.l + forces_.m + forces_.n)) 317 | { 318 | // apply the forces and torques to the joint 319 | link_->AddRelativeForce(GazeboVector(forces_.Fx, -forces_.Fy, -forces_.Fz)); 320 | link_->AddRelativeTorque(GazeboVector(forces_.l, -forces_.m, -forces_.n)); 321 | } 322 | } 323 | 324 | GZ_REGISTER_MODEL_PLUGIN(AircraftForcesAndMoments); 325 | } 326 | -------------------------------------------------------------------------------- /rosplane_sim/src/aircraft_truth.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 Gary Ellingson, MAGICC Lab, Brigham Young University, Provo, UT 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "rosplane_sim/aircraft_truth.h" 18 | 19 | namespace gazebo 20 | { 21 | 22 | AircraftTruth::AircraftTruth() : 23 | ModelPlugin(), 24 | nh_(nullptr), 25 | prev_sim_time_(0) 26 | {} 27 | 28 | 29 | AircraftTruth::~AircraftTruth() 30 | { 31 | GZ_COMPAT_DISCONNECT_WORLD_UPDATE_BEGIN(updateConnection_); 32 | if (nh_) 33 | { 34 | nh_->shutdown(); 35 | delete nh_; 36 | } 37 | } 38 | 39 | 40 | void AircraftTruth::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 41 | { 42 | model_ = _model; 43 | world_ = model_->GetWorld(); 44 | 45 | namespace_.clear(); 46 | 47 | /* 48 | * Connect the Plugin to the Robot and Save pointers to the various elements in the simulation 49 | */ 50 | if (_sdf->HasElement("namespace")) 51 | namespace_ = _sdf->GetElement("namespace")->Get(); 52 | else 53 | gzerr << "[gazebo_aircraft_truth] Please specify a namespace.\n"; 54 | nh_ = new ros::NodeHandle(namespace_); 55 | 56 | if (_sdf->HasElement("linkName")) 57 | link_name_ = _sdf->GetElement("linkName")->Get(); 58 | else 59 | gzerr << "[gazebo_aircraft_truth] Please specify a linkName of the truth plugin.\n"; 60 | link_ = model_->GetLink(link_name_); 61 | if (link_ == NULL) 62 | gzthrow("[gazebo_aircraft_truth] Couldn't find specified link \"" << link_name_ << "\"."); 63 | 64 | /* Load Params from Gazebo Server */ 65 | wind_speed_topic_ = nh_->param("windSpeedTopic", "gazebo/wind_speed"); 66 | truth_topic_ = nh_->param("truthTopic", "truth"); 67 | 68 | // Connect the update function to the simulation 69 | updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&AircraftTruth::OnUpdate, this, _1)); 70 | 71 | // Connect Subscribers 72 | true_state_pub_ = nh_->advertise(truth_topic_, 1); 73 | wind_speed_sub_ = nh_->subscribe(wind_speed_topic_, 1, &AircraftTruth::WindSpeedCallback, this); 74 | } 75 | 76 | // This gets called by the world update event. 77 | void AircraftTruth::OnUpdate(const common::UpdateInfo &_info) 78 | { 79 | 80 | sampling_time_ = _info.simTime.Double() - prev_sim_time_; 81 | prev_sim_time_ = _info.simTime.Double(); 82 | PublishTruth(); 83 | } 84 | 85 | void AircraftTruth::WindSpeedCallback(const geometry_msgs::Vector3 &wind) 86 | { 87 | wind_.N = wind.x; 88 | wind_.E = wind.y; 89 | wind_.D = wind.z; 90 | } 91 | 92 | 93 | void AircraftTruth::PublishTruth() 94 | { 95 | /* Get state information from Gazebo - convert to NED * 96 | * C denotes child frame, P parent frame, and W world frame. * 97 | * Further C_pose_W_P denotes pose of P wrt. W expressed in C.*/ 98 | rosplane_msgs::State msg; 99 | // Set origin values to zero by default 100 | msg.initial_lat = 0; 101 | msg.initial_lon = 0; 102 | msg.initial_alt = 0; 103 | 104 | GazeboPose W_pose_W_C = GZ_COMPAT_GET_WORLD_COG_POSE(link_); 105 | msg.position[0] = GZ_COMPAT_GET_X(GZ_COMPAT_GET_POS(W_pose_W_C)); // We should check to make sure that this is right 106 | msg.position[1] = -GZ_COMPAT_GET_Y(GZ_COMPAT_GET_POS(W_pose_W_C)); 107 | msg.position[2] = -GZ_COMPAT_GET_Z(GZ_COMPAT_GET_POS(W_pose_W_C)); 108 | GazeboVector euler_angles = GZ_COMPAT_GET_EULER(GZ_COMPAT_GET_ROT(W_pose_W_C)); 109 | msg.phi = GZ_COMPAT_GET_X(euler_angles); 110 | msg.theta = -GZ_COMPAT_GET_Y(euler_angles); 111 | msg.psi = -GZ_COMPAT_GET_Z(euler_angles); 112 | GazeboVector C_linear_velocity_W_C = GZ_COMPAT_GET_RELATIVE_LINEAR_VEL(link_); 113 | double u = GZ_COMPAT_GET_X(C_linear_velocity_W_C); 114 | double v = -GZ_COMPAT_GET_Y(C_linear_velocity_W_C); 115 | double w = -GZ_COMPAT_GET_Z(C_linear_velocity_W_C); 116 | msg.Vg = sqrt(pow(u, 2.0) + pow(v, 2.0) + pow(w, 2.0)); 117 | GazeboVector C_angular_velocity_W_C = GZ_COMPAT_GET_RELATIVE_ANGULAR_VEL(link_); 118 | msg.p = GZ_COMPAT_GET_X(C_angular_velocity_W_C); 119 | msg.q = -GZ_COMPAT_GET_Y(C_angular_velocity_W_C); 120 | msg.r = -GZ_COMPAT_GET_Z(C_angular_velocity_W_C); 121 | 122 | msg.wn = wind_.N; 123 | msg.we = wind_.E; 124 | 125 | // wind info is available in the wind_ struct 126 | double ur = u ;//- wind_.N; 127 | double vr = v ;//- wind_.E; 128 | double wr = w ;//- wind_.D; 129 | 130 | msg.Va = sqrt(pow(ur, 2.0) + pow(vr, 2.0) + pow(wr, 2.0)); 131 | msg.chi = atan2(msg.Va*sin(msg.psi), msg.Va*cos(msg.psi)); 132 | msg.alpha = atan2(wr , ur); 133 | msg.beta = asin(vr/msg.Va); 134 | 135 | msg.quat_valid = false; 136 | msg.quat[0] = u; 137 | msg.quat[1] = v; 138 | msg.quat[2] = w; 139 | 140 | msg.header.stamp.fromSec(GZ_COMPAT_GET_SIM_TIME(world_).Double()); 141 | msg.header.frame_id = 1; // Denotes global frame 142 | 143 | msg.psi_deg = fmod(GZ_COMPAT_GET_X(euler_angles), 2.0*M_PI)*180.0 / M_PI; //-360 to 360 144 | msg.psi_deg += (msg.psi_deg < -180.0 ? 360.0 : 0.0); 145 | msg.psi_deg -= (msg.psi_deg > 180.0 ? 360.0 : 0.0); 146 | msg.chi_deg = fmod(msg.chi, 2.0*M_PI)*180.0 / M_PI; //-360 to 360 147 | msg.chi_deg += (msg.chi_deg < -180.0 ? 360.0 : 0.0); 148 | msg.chi_deg -= (msg.chi_deg > 180.0 ? 360.0 : 0.0); 149 | 150 | true_state_pub_.publish(msg); 151 | } 152 | 153 | GZ_REGISTER_MODEL_PLUGIN(AircraftTruth); 154 | } 155 | -------------------------------------------------------------------------------- /rosplane_sim/xacro/aircraft_forces_and_moments.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 19 | 22 | ${parent_link} 23 | ${namespace} 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /rosplane_sim/xacro/aircraft_truth.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 10 | 11 | 12 | ${namespace} 13 | ${namespace}/base_link 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /rosplane_sim/xacro/fixedwing.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 51 | 52 | 53 | 54 | 57 | 58 | 59 | 60 | 63 | 64 | 65 | 66 | 67 | 70 | 71 | 72 | 73 | 74 | 77 | 78 | 79 | 80 | 81 | 84 | 85 | 86 | 87 | 97 | 98 | 99 | 110 | 111 | 112 | 113 | -------------------------------------------------------------------------------- /rosplane_sim/xacro/fixedwing_sil.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 51 | 52 | 53 | 54 | 55 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /rosplane_sim/xacro/meshes/byu_wing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/rosplane/78fe930224192a3031ee720d2fd1a669fa89d353/rosplane_sim/xacro/meshes/byu_wing.png --------------------------------------------------------------------------------