├── .gitignore ├── .gitmodules ├── README.md ├── roscopter ├── CMakeLists.txt ├── cfg │ └── Controller.cfg ├── include │ ├── controller │ │ ├── controller.h │ │ └── simple_pid.h │ ├── ekf │ │ ├── ekf.h │ │ ├── ekf_ros.h │ │ ├── ekf_rosbag.h │ │ ├── meas.h │ │ └── state.h │ └── roscopter │ │ └── eigen_helpers.h ├── launch │ ├── bag_gps.launch │ ├── gps.launch │ ├── mocap.launch │ └── mocap2ublox.launch ├── package.xml ├── params │ ├── mocap2ublox.yaml │ ├── multirotor.yaml │ ├── quadcopter.yaml │ ├── sub_params │ │ ├── controller.yaml │ │ ├── ekf.yaml │ │ └── rosflight_default_parameters.yml │ └── waypoints.yaml ├── scripts │ ├── commands_plotter.py │ ├── ekf_data.py │ ├── mocap2ublox.py │ ├── mocap2ublox_ros.py │ ├── plot_ekf.py │ ├── plot_window.py │ └── states_plotter.py └── src │ ├── controller │ ├── controller.cpp │ ├── controller_node.cpp │ └── simple_pid.cpp │ ├── ekf │ ├── dynamics.cpp │ ├── ekf.cpp │ ├── ekf_node.cpp │ ├── ekf_ros.cpp │ ├── ekf_rosbag.cpp │ ├── log.cpp │ ├── meas.cpp │ └── state.cpp │ └── waypoint_manager │ ├── velocity_commander.py │ ├── velocity_manager.py │ └── waypoint_manager.py ├── roscopter_msgs ├── CMakeLists.txt ├── msg │ ├── Command.msg │ └── PoseEuler.msg ├── package.xml └── srv │ ├── AddWaypoint.srv │ ├── ClearWaypoints.srv │ ├── Fly.srv │ ├── Hold.srv │ ├── Land.srv │ ├── ListWaypoints.srv │ ├── Release.srv │ ├── RemoveWaypoint.srv │ ├── ReturnToBase.srv │ └── SetWaypointsFromFile.srv ├── roscopter_pkgs ├── CMakeLists.txt └── package.xml ├── roscopter_sim ├── CMakeLists.txt ├── include │ └── roscopter_sim │ │ ├── common.h │ │ ├── gz_compat.h │ │ └── multirotor_forces_and_moments.h ├── launch │ ├── gazebo.launch │ ├── holodeck.launch │ └── sub_launch │ │ ├── spawn_mav_gazebo.launch │ │ └── spawn_mav_holodeck.launch ├── package.xml ├── params │ ├── gazebo │ │ ├── gazebo_world.yaml │ │ ├── multirotor_gazebo_roscopter_sim.yaml │ │ └── multirotor_gazebo_rosflight_sim.yaml │ └── holodeck │ │ ├── holodeck_world.yaml │ │ └── multirotor_holodeck.yaml ├── src │ └── multirotor_forces_and_moments.cpp └── xacro │ ├── multirotor_forces_and_moments.xacro │ ├── roscopter_sim.urdf.xacro │ └── rosflight_sim.urdf.xacro ├── roscopter_utils ├── CMakeLists.txt ├── include │ └── roscopter_utils │ │ ├── gnss.h │ │ ├── input_parser.h │ │ ├── logger.h │ │ ├── progress_bar.h │ │ └── yaml.h ├── lib │ └── geometry │ │ ├── .gitignore │ │ ├── .travis.yml │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── cmake │ │ └── geometryConfig.cmake.in │ │ ├── geometry-config.cmake │ │ ├── include │ │ └── geometry │ │ │ ├── cam.h │ │ │ ├── quat.h │ │ │ ├── support.h │ │ │ └── xform.h │ │ └── src │ │ └── test.cpp ├── package.xml └── src │ └── vimfly └── wiki_resources ├── ROScopterArchitecure.png ├── ROScopterLogo.png ├── ROScopter_control_modes.pdf ├── ROScopter_control_modes.png ├── ROSflightLogo.png ├── ROSplaneLogo.png └── rosflight_logo.png /.gitignore: -------------------------------------------------------------------------------- 1 | *.aux 2 | *.log 3 | *.fls 4 | *.fdb_latexmk 5 | *.synctex.gz 6 | *.swp 7 | *.autosave 8 | *.pdf 9 | roscopter/logs/ 10 | *.pyc 11 | .vscode/ 12 | .idea/ 13 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/.gitmodules -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ROScopter 2 | ========= 3 | 4 | This verison of ROScopter is no longer under development and no longer supported. 5 | ROScopter for ROS2 is currently under development and in alpha, you can find ROScopter [here](https://github.com/rosflight/roscopter). 6 | 7 | ------ Getting Started ------ 8 | 9 | Install the latest version of ROS on your computer. 10 | 11 | Make sure to install the following packages to your python environment. 12 | 13 | $ pip install pyyaml 14 | 15 | $ pip install numpy 16 | 17 | $ pip install rospkg 18 | 19 | $ pip install pycryptodomex 20 | 21 | $ pip install gnupg 22 | 23 | $ pip install empy 24 | 25 | If you would like to use the keyboard or a joypad to control the multirotor make sure to install pygame. 26 | 27 | $ pip install pygame 28 | 29 | Clone this repo (ROScopter) and the ROSflight (https://github.com/rosflight/rosflight) repo into your catkin_ws/src. 30 | 31 | To use the gazebo simulation package, clone the ROSflight_plugins repo (https://github.com/byu-magicc/rosflight_plugins). If you would like to use the keyboard as a RC controller, clone the rosflight_joy repo as well (https://github.com/rosflight/rosflight_joy). 32 | 33 | If you would like to use the rosflight_holodeck simulation package, clone the rosflight_holodeck repo (https://magiccvs.byu.edu/gitlab/lab/rosflight_holodeck) and follow installation instructions on the linked page. 34 | 35 | Next install catkin and create a catkin workspace. Build and source the catkin workspace by navigating to the workspace and running these commands. 36 | 37 | $ catkin_make 38 | 39 | $ source devel/setup.bash 40 | 41 | 42 | ----- ROScopter Sim ----- 43 | 44 | This simulation does not simulate the firmware (rosflight) and does not require RC input. It has the same API as the SIL stack, and can be useful for developing high-level algorithms, however it makes some simplifying assumptions in the dynamics which reduces the fidelity of the simulation. 45 | 46 | You can run the Gazebo simulator with 47 | 48 | $ roslaunch roscopter_sim gazebo.launch 49 | 50 | Make sure to push the play button in the Gazebo simulator. 51 | 52 | ----- Wiki ----- 53 | 54 | To get more help on how to setup roscopter visit the wiki (https://github.com/byu-magicc/roscopter/wiki). 55 | 56 | # TODO: # 57 | 58 | 1. Develop and document process noise std tuning methods. 59 | 60 | 2. Develop and document a tuning procedure for the multirotor. This includes tuning all the gain parameters for the flight vehicle. 61 | 62 | 3. Standardize the flight controller board. Board sensor suite needs to include an IMU, barometer and magnetometer. Current board and sensor combination is becoming obsolete. 63 | 64 | 4. Implement a trajectory follower control node. Should recieve inertial position, velocity, acceleration and heading, and output angular rate and throttle. 65 | 66 | 5. Develop and document a magnetometer calibration routine and incorporate magnetometer measurements to the estimator. This would enable accurate heading estimates at initialization and during hover flight. 67 | 68 | 6. Support an additional altitude sensor for landing. Possible options include sonar, laser, or camera. 69 | 70 | 7. Create a failsafe mode so the quadrotor can have a safe landing. Use cases include when RC connection is lost. 71 | 72 | 8. Improve flight performance in windy conditions. Most accurate solution is to incorporate wind estimation. This is possible without air sensors if you can estimate parameters of the flight vehicle such as drag and thrust coefficients. 73 | 74 | 9. Enable Roscopter to fly in reference to the vehicle 1 frame and body fixed frames as well as the inertial frame. 75 | -------------------------------------------------------------------------------- /roscopter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roscopter) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | dynamic_reconfigure 8 | roscopter_msgs 9 | rosflight_msgs 10 | rosflight_utils 11 | roscopter_utils 12 | std_msgs 13 | nav_msgs 14 | sensor_msgs 15 | rosbag 16 | roslib 17 | ) 18 | find_package(Eigen3 REQUIRED) 19 | find_package(yaml-cpp REQUIRED) 20 | find_package(inertial_sense QUIET) 21 | 22 | if (${inertial_sense_FOUND}) 23 | message(WARNING "found inertial sense library") 24 | add_definitions(-DINERTIAL_SENSE) 25 | else() 26 | message(WARNING "Did NOT find inertial sense library") 27 | endif() 28 | 29 | find_package(ublox QUIET) 30 | 31 | if (${ublox_FOUND}) 32 | message(WARNING "found ublox library") 33 | add_definitions(-DUBLOX) 34 | else() 35 | message(WARNING "Did NOT find ublox library") 36 | endif() 37 | 38 | generate_dynamic_reconfigure_options( 39 | cfg/Controller.cfg 40 | ) 41 | 42 | catkin_package( 43 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} 44 | CATKIN_DEPENDS roscpp rospy dynamic_reconfigure eigen_conversions 45 | roscopter_msgs rosflight_msgs rosflight_utils std_msgs nav_msgs sensor_msgs roslib 46 | DEPENDS EIGEN3 47 | ) 48 | 49 | add_definitions(-DROSCOPTER_DIR="${CMAKE_CURRENT_LIST_DIR}") 50 | 51 | ########### 52 | ## Build ## 53 | ########### 54 | 55 | include_directories(include 56 | ${EIGEN3_INCLUDE_DIRS} 57 | ${catkin_INCLUDE_DIRS} 58 | ) 59 | message (STATUS "catkin include dirs" ${catkin_INCLUDE_DIRS}) 60 | 61 | ## Declare cpp executables 62 | add_executable(controller 63 | src/controller/controller_node.cpp 64 | src/controller/controller.cpp 65 | include/controller/controller.h 66 | src/controller/simple_pid.cpp) 67 | add_dependencies(controller ${catkin_EXPORTED_TARGETS}) 68 | target_link_libraries(controller ${catkin_LIBRARIES}) 69 | 70 | add_library(ekf 71 | src/ekf/state.cpp 72 | src/ekf/dynamics.cpp 73 | src/ekf/ekf.cpp 74 | src/ekf/meas.cpp 75 | src/ekf/log.cpp 76 | ) 77 | target_include_directories(ekf PUBLIC include ${catkin_INCLUDE_DIRS}) 78 | target_link_libraries(ekf ${YAML_CPP_LIBRARIES} stdc++fs) 79 | 80 | add_library(ekf_ros src/ekf/ekf_ros.cpp) 81 | target_link_libraries(ekf_ros ekf ${catkin_LIBRARIES}) 82 | add_dependencies(ekf_ros ${catkin_EXPORTED_TARGETS}) 83 | 84 | add_executable(ekf_node src/ekf/ekf_node.cpp) 85 | target_link_libraries(ekf_node ekf_ros ${catkin_LIBRARIES}) 86 | add_dependencies(ekf_node ${catkin_EXPORTED_TARGETS}) 87 | 88 | add_executable(ekf_rosbag src/ekf/ekf_rosbag.cpp) 89 | target_link_libraries(ekf_rosbag ekf_ros ${catkin_LIBRARIES}) 90 | add_dependencies(ekf_rosbag ${catkin_EXPORTED_TARGETS}) 91 | -------------------------------------------------------------------------------- /roscopter/cfg/Controller.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roscopter" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("tau", double_t, 0, "Dirty Derivative Filter Constant", 0.0, 0, 1.0); 9 | gen.add("equilibrium_throttle", double_t, 0, "Throttle to hold equilibrium altitude (0. to 1.)", 0.0, 0, 1.0); 10 | 11 | vel = gen.add_group("Velocity") 12 | vel.add("x_dot_P", double_t, 0, "Forward Velocity (Vehicle 1 Frame) Proportional gain", 0.0, 0, 5.0); 13 | vel.add("x_dot_I", double_t, 0, "Forward Velocity (Vehicle 1 Frame) Integral gain", 0.0, 0, 5.0); 14 | vel.add("x_dot_D", double_t, 0, "Forward Velocity (Vehicle 1 Frame) Derivative gain", 0.1, 0, 5.0); 15 | 16 | vel.add("y_dot_P", double_t, 0, "Right Velocity (Vehicle 1 Frame) Proportional gain", 0.0, 0, 5.0); 17 | vel.add("y_dot_I", double_t, 0, "Right Velocity (Vehicle 1 Frame) Integral gain", 0, 0, 5.0); 18 | vel.add("y_dot_D", double_t, 0, "Right Velocity (Vehicle 1 Frame) Derivative gain", 0.0, 0, 5.0); 19 | 20 | vel.add("z_dot_P", double_t, 0, "Down Velocity (Vehicle 1 Frame) Proportional gain", 0.0, 0, 5.0); 21 | vel.add("z_dot_I", double_t, 0, "Down Velocity (Vehicle 1 Frame) Integral gain", 0, 0, 5.0); 22 | vel.add("z_dot_D", double_t, 0, "Down Velocity (Vehicle 1 Frame) Derivative gain", 0.0, 0, 5.0); 23 | 24 | pos = gen.add_group("Position") 25 | pos.add("north_P", double_t, 0, "North Position Proportional gain", 0.0, 0, 5.0); 26 | pos.add("north_I", double_t, 0, "North Position Integral gain", 0, 0, 5.0); 27 | pos.add("north_D", double_t, 0, "North Position Derivative gain", 0.0, 0, 5.0); 28 | 29 | pos.add("east_P", double_t, 0, "East Position Proportional gain", 0.0, 0, 5.0); 30 | pos.add("east_I", double_t, 0, "East Position Integral gain", 0, 0, 5.0); 31 | pos.add("east_D", double_t, 0, "East Position Derivative gain", 0.0, 0, 5.0); 32 | 33 | pos.add("down_P", double_t, 0, "Altitude Proportional gain", 0.0, 0, 5.0); 34 | pos.add("down_I", double_t, 0, "Altitude Integral gain", 0.0, 0, 5.0); 35 | pos.add("down_D", double_t, 0, "Altitude Derivative gain", 0.0, 0, 5.0); 36 | 37 | angle = gen.add_group("Angle") 38 | angle.add("psi_P", double_t, 0, "Yaw Angle Proportional gain", 0.0, 0, 25); 39 | angle.add("psi_I", double_t, 0, "Yaw Angle Integral gain", 0, 0, 25); 40 | angle.add("psi_D", double_t, 0, "Yaw Angle Derivative gain", 0, 0, 25); 41 | 42 | limits = gen.add_group("Limits") 43 | limits.add("max_roll", double_t, 0, "Maximum Roll Commanded by Controller (rad)", 0.0, 0, 1.5619); 44 | limits.add("max_pitch", double_t, 0, "Maximum Pitch Commanded by Controller (rad)", 0.0, 0, 1.5619); 45 | limits.add("max_yaw_rate", double_t, 0, "Maximum Yaw Rate Commanded by Controller (rad/s)", 0.0, 0, 6.825); 46 | limits.add("max_throttle", double_t, 0, "Maximum Throttle Commanded by Controller (normalized)", 0.0, 0, 1.0); 47 | limits.add("max_n_dot", double_t, 0, "Maximum North Velocity Commanded by Controller (m/s)", 0.0, 0, 45.0); 48 | limits.add("max_e_dot", double_t, 0, "Maximum East Velocity Commanded by Controller (m/s)", 0.0, 0, 45.0); 49 | limits.add("max_d_dot", double_t, 0, "Maximum Down Velocity Commanded by Controller (m/s)", 0.0, 0, 45.0); 50 | 51 | exit(gen.generate(PACKAGE, "roscopter", "Controller")) 52 | -------------------------------------------------------------------------------- /roscopter/include/controller/controller.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLLER_H 2 | #define CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | namespace controller 17 | { 18 | 19 | typedef struct 20 | { 21 | double pn; 22 | double pe; 23 | double pd; 24 | 25 | double phi; 26 | double theta; 27 | double psi; 28 | 29 | double u; 30 | double v; 31 | double w; 32 | 33 | double p; 34 | double q; 35 | double r; 36 | } state_t; 37 | 38 | typedef struct 39 | { 40 | double pn; 41 | double pe; 42 | double pd; 43 | 44 | double phi; 45 | double theta; 46 | double psi; 47 | 48 | double x_dot; 49 | double y_dot; 50 | double z_dot; 51 | 52 | double r; 53 | 54 | double ax; 55 | double ay; 56 | double az; 57 | 58 | double throttle; 59 | } command_t; 60 | 61 | typedef struct 62 | { 63 | double roll; 64 | double pitch; 65 | double yaw_rate; 66 | double throttle; 67 | double n_dot; 68 | double e_dot; 69 | double d_dot; 70 | } max_t; 71 | 72 | class Controller 73 | { 74 | 75 | public: 76 | 77 | Controller(); 78 | 79 | private: 80 | 81 | // Node handles, publishers, subscribers 82 | ros::NodeHandle nh_; 83 | ros::NodeHandle nh_private_; 84 | ros::NodeHandle nh_param_; 85 | 86 | // Publishers and Subscribers 87 | ros::Subscriber state_sub_; 88 | ros::Subscriber is_flying_sub_; 89 | ros::Subscriber cmd_sub_; 90 | ros::Subscriber status_sub_; 91 | 92 | ros::Publisher command_pub_; 93 | 94 | // Paramters 95 | double throttle_eq_; 96 | double mass_; 97 | double max_thrust_; 98 | double max_accel_xy_; 99 | double max_accel_z_; 100 | double min_altitude_; 101 | bool is_flying_; 102 | bool armed_; 103 | bool received_cmd_; 104 | 105 | // PID Controllers 106 | controller::SimplePID PID_x_dot_; 107 | controller::SimplePID PID_y_dot_; 108 | controller::SimplePID PID_z_dot_; 109 | controller::SimplePID PID_n_; 110 | controller::SimplePID PID_e_; 111 | controller::SimplePID PID_d_; 112 | controller::SimplePID PID_psi_; 113 | 114 | // Dynamic Reconfigure Hooks 115 | dynamic_reconfigure::Server _server; 116 | dynamic_reconfigure::Server::CallbackType _func; 117 | void reconfigure_callback(roscopter::ControllerConfig& config, 118 | uint32_t level); 119 | 120 | // Memory for sharing information between functions 121 | state_t xhat_ = {}; // estimate 122 | max_t max_ = {}; 123 | rosflight_msgs::Command command_; 124 | command_t xc_ = {}; // command 125 | double prev_time_; 126 | uint8_t control_mode_; 127 | 128 | // Functions 129 | void stateCallback(const nav_msgs::OdometryConstPtr &msg); 130 | void isFlyingCallback(const std_msgs::BoolConstPtr &msg); 131 | void cmdCallback(const roscopter_msgs::CommandConstPtr &msg); 132 | void statusCallback(const rosflight_msgs::StatusConstPtr &msg); 133 | 134 | void computeControl(double dt); 135 | void resetIntegrators(); 136 | void publishCommand(); 137 | double saturate(double x, double max, double min); 138 | 139 | }; 140 | } 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /roscopter/include/controller/simple_pid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Robert Leishman, 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 | /*! 33 | * \brief This file defines a simple PID controller to be used by other classes to implement a PID control loop 34 | * \author Robert Leishman 35 | * \date Dec. 2013 36 | */ 37 | 38 | #ifndef ROTOR_CONTROLLER_SIMPLE_PID_H 39 | #define ROTOR_CONTROLLER_SIMPLE_PID_H 40 | 41 | #include 42 | #include // included temporarily for debug statements 43 | 44 | namespace controller 45 | { 46 | /*! 47 | * \brief The simplePID class is a basic, tried and true PID controller. Only P (proportional) gains are 48 | * necessary, the I (integral) and D (derivative) default to zero. The I control is computed using a 49 | * first-order numerical integral of the error, the derivative is the numerical first-order derivative 50 | * of the error. Due to these crude integration techniques, it is best if the control be computed fast 51 | * (i.e. small dt). 52 | */ 53 | class SimplePID 54 | { 55 | public: 56 | /*! 57 | * \brief SimplePID is the basic initializer; 58 | */ 59 | SimplePID(); 60 | 61 | /*! 62 | * \brief SimplePID initializes the class. 63 | * \param p the proportional controller gain (required) 64 | * \param i the integral controller gain (defaults to zero) 65 | * \param d the derivative controller gain (defaults to zero) 66 | * \param imin the min value accepted in the output of the integral control 67 | * \param imax the max value accepted in the output of the integral control (saturation for integrator windup) 68 | * \param tau band limited differentiator to reduce noise 69 | */ 70 | SimplePID(double p, double i = 0.0, double d = 0.0, double max = DBL_MAX, double min = -DBL_MAX, double tau = 0.15); 71 | 72 | /*! 73 | * \brief computePID computes the PID control for the given error and timestep (since the last control was computed!) 74 | * \param p_error is the "position" error (or whatever variable you are controlling) 75 | * \param dt is the timestep since the last control was computed. 76 | * \param x_dot derivative of current state (optional) 77 | * \return the control command 78 | */ 79 | double computePID(double desired, double current, double dt, double x_dot = INFINITY); 80 | 81 | /*! 82 | * \brief setgains is used to set the gains for a controller after it's been initialized. It will rewrite 83 | * whatever is already there! 84 | * \param p the proportional controller gain (required) 85 | * \param i the integral controller gain (defaults to zero) 86 | * \param d the derivative controller gain (defaults to zero) 87 | * \param tau band limited differentiator to reduce noise 88 | */ 89 | void setGains(double p, double i = 0.0, double d = 0.0, double tau = 0.15, double max_u = DBL_MAX, double min_u = -DBL_MAX); 90 | 91 | /*! 92 | * \brief setgains is used to set the gains for a controller after it's been initialized. It will rewrite 93 | * whatever is already there! 94 | * \param max the largest output allowed (integrator anti-windup will kick in at this value as well) 95 | * \param min the smallest output allowed (also activates integrator anti-windup 96 | */ 97 | void setLimits(double max, double min); 98 | 99 | /*! 100 | * \brief clearIntegrator allows you to clear the integrator, in case of integrator windup. 101 | */ 102 | void clearIntegrator() 103 | { 104 | integrator_ = 0.0; 105 | } 106 | 107 | protected: 108 | double kp_; //!< the proportional gain 109 | double ki_; //!< the integral gain (zero if you don't want integral control) 110 | double kd_; //!< the derivative gain (zero if you don't want derivative control) 111 | double integrator_; //!< the integral of p_error 112 | double differentiator_; //!< used for noise reduced differentiation 113 | double last_error_; //!< the last p_error, for computing the derivative; 114 | double last_state_; //!< the last state, for computing the derivative; 115 | double tau_; //!< the noise reduction term for the derivative 116 | double max_; //!< Maximum Output 117 | double min_; //!< Minimum Output 118 | 119 | /*! 120 | * \brief saturate saturates the variable val 121 | * \param val the parameter to saturate (makes a copy) 122 | * \param min the minimum value 123 | * \param max the max value 124 | * \return the saturated (if necessary) value 125 | */ 126 | inline double saturate(double val, double &min, double &max) 127 | { 128 | if (val > max) 129 | val = max; 130 | else if (val < min) 131 | val = min; 132 | return val; 133 | } 134 | }; 135 | } 136 | 137 | #endif // ROTOR_CONTROLLER_SIMPLE_PID_H 138 | -------------------------------------------------------------------------------- /roscopter/include/ekf/ekf.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "ekf/state.h" 10 | #include "ekf/meas.h" 11 | #include "roscopter_utils/logger.h" 12 | #include "roscopter_utils/gnss.h" 13 | #include "roscopter_utils/yaml.h" 14 | 15 | /// TO ADD A NEW MEASUREMENT 16 | /// Add a new Meas type to the meas.h header file and meas.cpp 17 | /// Add a new callback like mocapCallback()... 18 | /// Add a new update function like mocapUpdate()... 19 | /// Add new cases to the update function 20 | /// Profit. 21 | 22 | /// TO ADD A NEW STATE 23 | /// Add an index in the ErrorState and State objects in state.cpp/state.h 24 | /// Make sure the SIZE enums are correct 25 | /// Add relevant Jacobians and Dynamics to measurement update functions and dynamics 26 | /// Profit. 27 | 28 | namespace roscopter 29 | { 30 | 31 | #define CHECK_NAN(mat) \ 32 | if ((mat.array() != mat.array()).any())\ 33 | {\ 34 | std::cout << #mat << std::endl << mat << std::endl;\ 35 | throw std::runtime_error(#mat " Has NaNs" + std::to_string(__LINE__));\ 36 | } 37 | 38 | #define PRINTMAT(mat) std::cout << #mat << std::endl << mat << std::endl; 39 | 40 | namespace ekf 41 | { 42 | 43 | 44 | static const Eigen::Vector3d gravity = (Eigen::Vector3d() << 0, 0, 9.80665).finished(); 45 | 46 | class EKF 47 | { 48 | public: 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | 51 | static const dxMat I_BIG; 52 | 53 | EKF(); 54 | ~EKF(); 55 | 56 | State& x() { return xbuf_.x(); } // The current state object 57 | const State& x() const { return xbuf_.x(); } 58 | dxMat& P() { return xbuf_.P(); } // The current covariance 59 | const dxMat& P() const { return xbuf_.P(); } 60 | 61 | void initialize(double t); 62 | void load(const std::string& filename, const std::string& param_dictionary); 63 | void initLog(const std::string& filename, const std::string& param_dictionary); 64 | 65 | void run(); 66 | void update(const meas::Base *m); 67 | 68 | void setArmed() { armed_ = true; } 69 | void setDisarmed() { armed_ = false; } 70 | void setUseMocap(bool use_mocap) {use_mocap_ = use_mocap; } 71 | void setUseBaro(bool use_baro) {use_baro_ = use_baro; } 72 | void setUseGNSS(bool use_gnss) {use_gnss_ = use_gnss; } 73 | void setUseZeroVel(bool use_zero_vel) {use_zero_vel_ = use_zero_vel; } 74 | 75 | bool refLlaSet() { return ref_lla_set_; } 76 | 77 | void setGroundTempPressure(const double& temp, const double& press); 78 | bool groundTempPressSet() { return (ground_pressure_ != 0) && (ground_temperature_ != 0); } 79 | 80 | bool isFlying() { return is_flying_; } 81 | void checkIsFlying(); 82 | 83 | bool measUpdate(const Eigen::VectorXd &res, const Eigen::MatrixXd &R, const Eigen::MatrixXd &H); 84 | void propagate(const double& t, const Vector6d &imu, const Matrix6d &R); 85 | void dynamics(const State &x, const Vector6d& u, ErrorState &dx, bool calc_jac=false); 86 | void errorStateDynamics(const State& x, const ErrorState& dx, const Vector6d& u, 87 | const Vector6d& eta, ErrorState& dxdot); 88 | 89 | meas::MeasSet::iterator getOldestNewMeas(); 90 | void imuCallback(const double& t, const Vector6d& z, const Matrix6d& R); 91 | void baroCallback(const double& t, const double& z, const double& R, 92 | const double& temp); 93 | void rangeCallback(const double& t, const double& z, const double& R); 94 | void gnssCallback(const double& t, const Vector6d& z, const Matrix6d& R); 95 | void mocapCallback(const double& t, const xform::Xformd& z, const Matrix6d& R); 96 | 97 | void baroUpdate(const meas::Baro &z); 98 | void rangeUpdate(const meas::Range &z); 99 | void gnssUpdate(const meas::Gnss &z); 100 | void mocapUpdate(const meas::Mocap &z); 101 | void zeroVelUpdate(double t); 102 | 103 | void setRefLla(Eigen::Vector3d ref_lla); 104 | 105 | void cleanUpMeasurementBuffers(); 106 | 107 | void initLog(); 108 | void logState(); 109 | void logCov(); 110 | enum { 111 | LOG_STATE, 112 | LOG_COV, 113 | LOG_GNSS_RES, 114 | LOG_MOCAP_RES, 115 | LOG_ZERO_VEL_RES, 116 | LOG_BARO_RES, 117 | LOG_RANGE_RES, 118 | LOG_IMU, 119 | LOG_LLA, 120 | LOG_REF, 121 | NUM_LOGS 122 | }; 123 | std::vector log_names_ { 124 | "state", 125 | "cov", 126 | "gnss_res", 127 | "mocap_res", 128 | "zero_vel_res", 129 | "baro_res", 130 | "range_res", 131 | "imu", 132 | "lla", 133 | "ref" 134 | }; 135 | bool enable_log_; 136 | std::vector logs_; 137 | std::string log_prefix_; 138 | 139 | double is_flying_threshold_; 140 | bool enable_arm_check_; 141 | bool is_flying_; 142 | bool armed_; 143 | 144 | // Constants 145 | xform::Xformd x0_; 146 | Eigen::Vector3d p_b2g_; 147 | xform::Xformd x_e2I_; 148 | quat::Quatd q_n2I_; 149 | Eigen::Matrix4d R_zero_vel_; 150 | 151 | bool ref_lla_set_; 152 | double ref_lat_radians_; 153 | double ref_lon_radians_; 154 | double P0_yaw_; 155 | double ground_pressure_; 156 | double ground_temperature_; 157 | bool update_baro_; 158 | double update_baro_vel_thresh_; 159 | 160 | // Matrix Workspace 161 | dxMat A_; 162 | dxMat Qx_; 163 | Matrix6d Qu_; 164 | dxuMat B_; 165 | dxuMat K_; 166 | ErrorState dx_; 167 | 168 | // Partial Update 169 | dxVec lambda_vec_; 170 | dxMat lambda_mat_; 171 | 172 | // State buffer 173 | StateBuf xbuf_; 174 | 175 | // Measurement Buffers 176 | bool use_mocap_; 177 | bool use_baro_; 178 | bool use_range_; 179 | bool use_gnss_; 180 | bool use_zero_vel_; 181 | bool enable_out_of_order_; 182 | bool enable_partial_update_; 183 | meas::MeasSet meas_; 184 | std::deque> imu_meas_buf_; 185 | std::deque> mocap_meas_buf_; 186 | std::deque> gnss_meas_buf_; 187 | std::deque> zv_meas_buf_; 188 | }; 189 | 190 | } 191 | 192 | } 193 | -------------------------------------------------------------------------------- /roscopter/include/ekf/ekf_ros.h: -------------------------------------------------------------------------------- 1 | // BSD 3-Clause License 2 | // 3 | // Copyright (c) 2017, James Jackson, BYU MAGICC Lab, Provo UT 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // 9 | // * Redistributions of source code must retain the above copyright notice, this 10 | // list of conditions and the following disclaimer. 11 | // 12 | // * Redistributions in binary form must reproduce the above copyright notice, 13 | // this list of conditions and the following disclaimer in the documentation 14 | // and/or other materials provided with the distribution. 15 | // 16 | // * Neither the name of the copyright holder nor the names of its 17 | // contributors may be used to endorse or promote products derived from 18 | // this software without specific prior written permission. 19 | // 20 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | 32 | #pragma once 33 | 34 | 35 | #include "ekf.h" 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #ifdef UBLOX 55 | #include "ublox/PosVelEcef.h" 56 | #endif 57 | 58 | #ifdef INERTIAL_SENSE 59 | #include "inertial_sense/GPS.h" 60 | #endif 61 | 62 | namespace roscopter::ekf 63 | { 64 | class EKF_ROS 65 | { 66 | public: 67 | 68 | EKF_ROS(); 69 | ~EKF_ROS(); 70 | void init(const std::string& param_file, const std::string& param_dictionary); 71 | void initROS(); 72 | 73 | void imuCallback(const sensor_msgs::ImuConstPtr& msg); 74 | void baroCallback(const rosflight_msgs::BarometerConstPtr& msg); 75 | void rangeCallback(const sensor_msgs::RangeConstPtr& msg); 76 | void poseCallback(const geometry_msgs::PoseStampedConstPtr &msg); 77 | void odomCallback(const nav_msgs::OdometryConstPtr &msg); 78 | void gnssCallback(const rosflight_msgs::GNSSConstPtr& msg); 79 | void mocapCallback(const ros::Time& time, const xform::Xformd &z); 80 | void statusCallback(const rosflight_msgs::StatusConstPtr& msg); 81 | 82 | #ifdef UBLOX 83 | void gnssCallbackUblox(const ublox::PosVelEcefConstPtr& msg); 84 | #endif 85 | 86 | #ifdef INERTIAL_SENSE 87 | void gnssCallbackInertialSense(const inertial_sense::GPSConstPtr& msg); 88 | #endif 89 | 90 | 91 | private: 92 | EKF ekf_; 93 | 94 | ros::Time last_imu_update_; 95 | 96 | ros::NodeHandle nh_; 97 | ros::NodeHandle nh_private_; 98 | ros::NodeHandle nh_param_; 99 | 100 | ros::Subscriber imu_sub_; 101 | ros::Subscriber baro_sub_; 102 | ros::Subscriber pose_sub_; 103 | ros::Subscriber odom_sub_; 104 | ros::Subscriber gnss_sub_; 105 | ros::Subscriber status_sub_; 106 | ros::Subscriber range_sub_; 107 | 108 | ros::Publisher odometry_pub_; 109 | ros::Publisher euler_pub_; 110 | ros::Publisher imu_bias_pub_; 111 | ros::Publisher is_flying_pub_; 112 | 113 | sensor_msgs::Imu imu_bias_msg_; 114 | nav_msgs::Odometry odom_msg_; 115 | geometry_msgs::Vector3Stamped euler_msg_; 116 | std_msgs::Bool is_flying_msg_; 117 | 118 | #ifdef UBLOX 119 | ros::Subscriber ublox_gnss_sub_; 120 | #endif 121 | 122 | #ifdef INERTIAL_SENSE 123 | ros::Subscriber is_gnss_sub_; 124 | #endif 125 | 126 | std::mutex ekf_mtx_; 127 | 128 | bool imu_init_ = false; 129 | bool truth_init_ = false; 130 | 131 | bool use_odom_; 132 | bool use_pose_; 133 | 134 | bool ros_initialized_ = false; 135 | 136 | bool is_flying_ = false; 137 | bool armed_ = false; 138 | ros::Time time_took_off_; 139 | ros::Time start_time_; 140 | 141 | Vector6d imu_; 142 | 143 | Matrix6d imu_R_; 144 | Matrix6d mocap_R_; 145 | double baro_R_; 146 | double range_R_; 147 | 148 | bool manual_gps_noise_; 149 | double gps_horizontal_stdev_; 150 | double gps_vertical_stdev_; 151 | double gps_speed_stdev_; 152 | 153 | void publishEstimates(const sensor_msgs::ImuConstPtr &msg); 154 | }; 155 | 156 | } 157 | 158 | 159 | 160 | 161 | -------------------------------------------------------------------------------- /roscopter/include/ekf/ekf_rosbag.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "ekf/ekf_ros.h" 8 | 9 | #include "roscopter_utils/yaml.h" 10 | #include "roscopter_utils/logger.h" 11 | #include "roscopter_utils/input_parser.h" 12 | #include "roscopter_utils/progress_bar.h" 13 | #include "roscopter_utils/gnss.h" 14 | 15 | namespace roscopter::ekf 16 | { 17 | 18 | class ROSbagParser 19 | { 20 | public: 21 | ROSbagParser(int argc, char** argv); 22 | 23 | void getArgs(int argc, char** argv); 24 | void loadParams(); 25 | void displayHelp(); 26 | void openBag(); 27 | void parseBag(); 28 | 29 | private: 30 | void odomCB(const nav_msgs::OdometryConstPtr& msg); 31 | void poseCB(const geometry_msgs::PoseStampedConstPtr& msg); 32 | void gnssCB(const rosflight_msgs::GNSSConstPtr& msg); 33 | 34 | rosbag::Bag bag_; 35 | rosbag::View* view_; 36 | std::string bag_filename_; 37 | std::string param_filename_; 38 | std::string param_namespace_; 39 | std::string log_prefix_; 40 | double start_; 41 | double duration_; 42 | double end_; 43 | bool got_imu_; 44 | 45 | ros::Time bag_start_; 46 | ros::Time bag_duration_; 47 | ros::Time bag_end_; 48 | 49 | EKF_ROS ekf_; 50 | 51 | std::string status_topic_; 52 | std::string imu_topic_; 53 | std::string baro_topic_; 54 | std::string range_topic_; 55 | std::string pose_topic_; 56 | std::string odom_topic_; 57 | std::string gnss_topic_; 58 | std::string ublox_gnss_topic_; 59 | std::string inertial_sense_gnss_topic_; 60 | 61 | Matrix6d imu_R_; 62 | Matrix6d mocap_R_; 63 | Matrix6d gnss_R_; 64 | 65 | Logger lla_log_; 66 | Logger truth_log_; 67 | Logger imu_log_; 68 | }; 69 | 70 | } 71 | 72 | -------------------------------------------------------------------------------- /roscopter/include/ekf/meas.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | namespace roscopter 11 | { 12 | 13 | namespace ekf 14 | { 15 | 16 | namespace meas 17 | { 18 | 19 | struct Base 20 | { 21 | Base(); 22 | virtual ~Base() = default; 23 | enum 24 | { 25 | BASE, 26 | GNSS, 27 | IMU, 28 | BARO, 29 | RANGE, 30 | MOCAP, 31 | ZERO_VEL 32 | }; 33 | double t; 34 | int type; 35 | bool handled; 36 | std::string Type() const; 37 | }; 38 | 39 | typedef std::multiset> MeasSet; 40 | bool basecmp(const Base *a, const Base *b); 41 | 42 | struct Gnss : public Base 43 | { 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | Gnss(double _t, const Vector6d& _z, const Matrix6d& _R); 46 | Vector6d z; 47 | Eigen::Map p; 48 | Eigen::Map v; 49 | Matrix6d R; 50 | }; 51 | 52 | struct Imu : public Base 53 | { 54 | enum 55 | { 56 | A = 0, 57 | W = 3 58 | }; 59 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 60 | Imu(double _t, const Vector6d& _z, const Matrix6d& _R); 61 | Vector6d z; 62 | Matrix6d R; 63 | }; 64 | 65 | struct Baro : public Base 66 | { 67 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 68 | Baro(double _t, const double& _z, const double& _R, const double& _temp); 69 | Eigen::Matrix z; 70 | Eigen::Matrix R; 71 | double temp; 72 | }; 73 | 74 | struct Range : public Base 75 | { 76 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 77 | Range(double _t, const double& _z, const double& _R); 78 | Eigen::Matrix z; 79 | Eigen::Matrix R; 80 | }; 81 | 82 | struct Mocap : public Base 83 | { 84 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 85 | Mocap(double _t, const xform::Xformd& _z, const Matrix6d& _R); 86 | xform::Xformd z; 87 | Matrix6d R; 88 | }; 89 | 90 | struct ZeroVel: public Base 91 | { 92 | ZeroVel(double _t); 93 | }; 94 | 95 | } 96 | 97 | } 98 | 99 | } 100 | 101 | -------------------------------------------------------------------------------- /roscopter/include/ekf/state.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace roscopter 7 | { 8 | 9 | namespace ekf 10 | { 11 | 12 | 13 | class ErrorState 14 | { 15 | public: 16 | enum { 17 | DX = 0, 18 | DP = 0, 19 | DQ = 3, 20 | DV = 6, 21 | DBA = 9, 22 | DBG = 12, 23 | DBB = 15, 24 | DREF = 16, 25 | NDX = 17, 26 | SIZE = 17 27 | }; 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | Eigen::Matrix arr; 30 | Eigen::Map x; 31 | Eigen::Map p; 32 | Eigen::Map q; 33 | Eigen::Map v; 34 | Eigen::Map ba; 35 | Eigen::Map bg; 36 | double& bb; // bias for barometer measurements 37 | double& ref; // reference global altitude of NED frame 38 | 39 | ErrorState(); 40 | ErrorState(const ErrorState& obj); 41 | ErrorState& operator=(const ErrorState& obj); 42 | ErrorState operator*(const double& s) const; 43 | ErrorState operator/(const double& s) const; 44 | ErrorState& operator*=(const double& s); 45 | ErrorState operator+(const ErrorState& obj) const; 46 | ErrorState operator-(const ErrorState& obj) const; 47 | ErrorState operator+(const Eigen::Matrix& obj) const; 48 | ErrorState operator-(const Eigen::Matrix& obj) const; 49 | ErrorState& operator+=(const Eigen::Matrix& obj); 50 | ErrorState& operator-=(const Eigen::Matrix& obj); 51 | ErrorState& operator+=(const ErrorState& obj); 52 | ErrorState& operator-=(const ErrorState& obj); 53 | 54 | static ErrorState Random() 55 | { 56 | ErrorState x; 57 | x.arr.setRandom(); 58 | return x; 59 | } 60 | 61 | static ErrorState Zero() 62 | { 63 | ErrorState x; 64 | x.arr.setZero(); 65 | return x; 66 | } 67 | }; 68 | 69 | class State 70 | { 71 | public: 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | 74 | enum { 75 | T = 0, 76 | X = 1, // for Xform access (p, q) 77 | P = 1, 78 | Q = 4, 79 | V = 8, 80 | BA = 11, 81 | BG = 14, 82 | BB = 17, 83 | REF = 18, 84 | A = 19, 85 | W = 22, 86 | NX = 18, // number of states 87 | SIZE = 1 + NX + 6 88 | }; 89 | Eigen::Matrix arr; 90 | 91 | Eigen::Map imu; // IMU measurement at current time 92 | Eigen::Map a; 93 | Eigen::Map w; 94 | 95 | double& t; // Time of current state 96 | xform::Xformd x; 97 | Eigen::Map p; 98 | quat::Quatd q; 99 | Eigen::Map v; 100 | Eigen::Map ba; 101 | Eigen::Map bg; 102 | double& bb; // barometer pressure bias 103 | double& ref; // reference global altitude of NED frame 104 | 105 | State(); 106 | State(const State& other); 107 | State& operator=(const State& obj); 108 | 109 | static State Random() 110 | { 111 | State x; 112 | x.arr.setRandom(); 113 | x.x = xform::Xformd::Random(); 114 | return x; 115 | } 116 | 117 | static State Identity() 118 | { 119 | State out; 120 | out.x = xform::Xformd::Identity(); 121 | out.v.setZero(); 122 | out.ba.setZero(); 123 | out.bg.setZero(); 124 | return out; 125 | } 126 | 127 | State operator+(const ErrorState &delta) const; 128 | State operator+(const Eigen::Matrix &delta) const; 129 | State& operator+=(const ErrorState &delta); 130 | State& operator+=(const Eigen::VectorXd& dx); 131 | ErrorState operator-(const State &x2) const; 132 | }; 133 | 134 | typedef Eigen::Matrix dxMat; 135 | typedef Eigen::Matrix dxVec; 136 | typedef Eigen::Matrix dxuMat; 137 | typedef Eigen::Matrix duMat; 138 | 139 | 140 | class StateBuf 141 | { 142 | public: 143 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 144 | 145 | struct Snapshot 146 | { 147 | State x; 148 | dxMat P; 149 | }; 150 | 151 | std::vector> buf; // circular buffer 152 | int head; 153 | int tail; 154 | int size; 155 | 156 | StateBuf(int size); 157 | State &x(); 158 | const State &x() const; 159 | dxMat &P(); 160 | const dxMat &P() const; 161 | 162 | Snapshot& next(); 163 | Snapshot& begin(); 164 | 165 | void advance(); 166 | bool rewind(double t); 167 | }; 168 | 169 | } 170 | 171 | } 172 | -------------------------------------------------------------------------------- /roscopter/include/roscopter/eigen_helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | template 8 | bool vectorToMatrix(Eigen::MatrixBase& mat, std::vector vec) 9 | { 10 | ROS_ASSERT(vec.size() == mat.rows()*mat.cols()); 11 | if(vec.size() != mat.rows()*mat.cols()) 12 | return false; 13 | for(unsigned i=0; i < mat.rows(); i++) 14 | { 15 | for(unsigned j=0; j < mat.cols(); j++) 16 | { 17 | mat(i,j) = vec[mat.cols()*i+j]; 18 | } 19 | } 20 | return true; 21 | } 22 | 23 | template 24 | void importMatrixFromParamServer(ros::NodeHandle nh, Eigen::MatrixBase& mat, std::string param) 25 | { 26 | std::vector vec; 27 | ROS_FATAL_COND(!nh.getParam(param, vec), "Could not find %s/%s on server.",nh.getNamespace().c_str(),param.c_str()); 28 | ROS_FATAL_COND(!vectorToMatrix(mat,vec), "Param %s/%s is the wrong size - param is %d (total) while matrix is %dx%d", 29 | nh.getNamespace().c_str(),param.c_str(), (int)vec.size(), (int)mat.rows(), (int)mat.cols()); 30 | } 31 | -------------------------------------------------------------------------------- /roscopter/launch/bag_gps.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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /roscopter/launch/gps.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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /roscopter/launch/mocap.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 | 43 | 44 | -------------------------------------------------------------------------------- /roscopter/launch/mocap2ublox.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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /roscopter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roscopter 4 | 0.0.0 5 | The roscopter package 6 | 7 | James Jackson 8 | 9 | TODO 10 | 11 | catkin 12 | ament_cmake 13 | 14 | 15 | 16 | 17 | 18 | nav_msgs 19 | std_msgs 20 | sensor_msgs 21 | roscpp 22 | rospy 23 | roscopter_msgs 24 | rosflight_msgs 25 | rosflight_utils 26 | eigen_conversions 27 | dynamic_reconfigure 28 | roslib 29 | 30 | message_generation 31 | 32 | message_runtime 33 | message_runtime 34 | 35 | 36 | 37 | 38 | 39 | catkin 40 | ament_cmake 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /roscopter/params/mocap2ublox.yaml: -------------------------------------------------------------------------------- 1 | #This param file is used in mocap2ublox node. 2 | #It converts motion capture date to a virtual set of ublox data 3 | 4 | mocap2ublox: { 5 | 6 | ublox_frequency: 5.0, #hz 7 | 8 | #Should they be less by a factor of 10? 9 | global_horizontal_accuracy: 0.4, #standard deviations 10 | global_vertical_accuracy: 0.6, 11 | global_speed_accuracy: 0.4, 12 | relative_horizontal_accuracy: 0.02, 13 | relative_vertical_accuracy: 0.06, 14 | relative_speed_accuracy: 0.02, 15 | 16 | noise_on: True, 17 | 18 | ref_lla: [40.267320, -111.635629, 1387.0], 19 | 20 | sigma_rover_pos: 5.0, #TODO: tune this 21 | sigma_rover_vel: 5.0, #TODO: tune this 22 | 23 | lpf_on: True, #low pass filter of measurements 24 | 25 | A: 6378137.0, # WGS-84 Earth semimajor axis (m), from ekf_ros.cpp 26 | B: 6356752.314245, # Derived Earth semiminor axis (m), from ekf_ros.cpp 27 | 28 | } -------------------------------------------------------------------------------- /roscopter/params/multirotor.yaml: -------------------------------------------------------------------------------- 1 | ### Controller Parameters 2 | 3 | controller: { 4 | 5 | x_dot_P: 0.5, 6 | x_dot_I: 0.0, 7 | x_dot_D: 0.0, 8 | 9 | y_dot_P: 0.5, 10 | y_dot_I: 0.0, 11 | y_dot_D: 0.0, 12 | 13 | z_dot_P: 1.0, 14 | z_dot_I: 0.0, 15 | z_dot_D: 0.0, 16 | 17 | north_P: 0.45, 18 | north_I: 0.0, 19 | north_d: 0.1, 20 | 21 | east_P: 0.45, 22 | east_I: 0.0, 23 | east_d: 0.1, 24 | 25 | down_P: 2.0, 26 | down_I: 0.7, 27 | down_D: 0.1, 28 | 29 | psi_P: 1.0, 30 | psi_I: 0.0, 31 | psi_D: 0.0, 32 | 33 | tau: 0.05, 34 | 35 | equilibrium_throttle: 0.5, 36 | 37 | max_roll: 0.196, 38 | max_pitch: 0.196, 39 | max_yaw_rate: 0.785, 40 | max_throttle: 0.85, 41 | max_n_dot: 1.5, 42 | max_e_dot: 1.5, 43 | max_d_dot: 1.25, 44 | 45 | min_altitude: 0.5 46 | 47 | } 48 | 49 | estimator: { 50 | ## Post-Process Information 51 | bag_name: "~/roscopter_ws/src/bags/test.bag", 52 | status_topic: "/status", 53 | imu_topic: "/imu/data", 54 | baro_topic: "/serial/baro", 55 | range_topic: "/serial/laser", 56 | pose_topic: "", 57 | odom_topic: "", 58 | gnss_topic: "", 59 | ublox_gnss_topic: "", 60 | inertial_sense_gnss_topic: "/ins/gps", 61 | start_time: 0, 62 | duration: 1000, 63 | ## Is_flying checks 64 | enable_arm_check: false, # require armed before checking for is_flying 65 | is_flying_threshold: 10.5, # if accel measurement exceed this magnitude, set is_flying 66 | update_baro_velocity_threshold: 0.5, 67 | ## Logging 68 | log_prefix: "/tmp/roscopter/ekf/", 69 | enable_log: true, 70 | ## Measurement Processing 71 | enable_partial_update: true, 72 | enable_out_of_order: false, # TODO this isnt fully implemented 73 | use_mocap: false, 74 | use_gnss: true, 75 | use_baro: true, 76 | use_range: false, 77 | use_zero_vel: true, #publishes zeros for the estimator until the quad starts flying, prevents drift before flight 78 | R_zero_vel: [0.001, 0.001, 0.001, # vel 79 | 0.001], # Altitude 80 | ## Initial Uncertainty 81 | P0: [10.0, 10.0, 10.0, # pos 82 | 0.01, 0.01, 0.01, # att 83 | 0.001, 0.001, 0.001, # vel 84 | 0.01, 0.01, 0.01, # accel bias 85 | 0.001, 0.001, 0.001, # gyro bias 86 | 9.0, # baro bias 87 | 100.0], # reference altitude 88 | ## Additive Process Noise 89 | Qx: [0.000, 0.000, 0.000, # pos 90 | 0.000, 0.000, 0.000, # att 91 | 0.00001, 0.00001, 0.00001, # vel 92 | 0.0001, 0.0001, 0.0001, # accel bias 93 | 0.0001, 0.0001, 0.0001, # gyro bias 94 | 0.000001, # baro bias 95 | 1.0], # reference altitude 96 | ## Lambda (Partial Update) 97 | # 1.0 = use full update 98 | # 0.0 = do not update at all 99 | lambda: [1.0, 1.0, 1.0, # pos 100 | 1.0, 1.0, 1.0, # att 101 | 1.0, 1.0, 1.0, # vel 102 | 0.1, 0.1, 0.1, # accel bias 103 | 0.1, 0.1, 0.1, # gyro bias 104 | 0.5, # baro bias 105 | 0.3], # reference altitude 106 | # Constant Parameeters 107 | p_b2g: [0, 0, 0], # position from body frame to gps frame 108 | # Initial State 109 | manual_ref_lla: false, # Use manual ref_lla or use first lla from gps 110 | ref_lla: [40.267320, -111.635629, 1387.0], 111 | ref_heading: 0.0, # reference heading for inertial frame (from north) 112 | x0: [0, 0, 0, # pos 113 | 1, 0, 0, 0], # att 114 | ## Sensor Noise 115 | #imu 116 | accel_noise_stdev: 0.5, 117 | gyro_noise_stdev: 0.02, 118 | #Mocap 119 | position_noise_stdev: 0.01, 120 | attitude_noise_stdev: 0.01, 121 | #barometer 122 | baro_pressure_noise_stdev: 2.0, 123 | #range sensor 124 | range_noise_stdev: 0.1, 125 | #gps 126 | manual_gps_noise: false, # choose noise params for GPS or use the values reported from the message topic 127 | gps_horizontal_stdev: 0.021, 128 | gps_vertical_stdev: 0.04, 129 | gps_speed_stdev: 0.2 130 | } 131 | 132 | -------------------------------------------------------------------------------- /roscopter/params/quadcopter.yaml: -------------------------------------------------------------------------------- 1 | 2 | ### Controller Parameters 3 | 4 | controller: { 5 | 6 | x_dot_P: 0.5, 7 | x_dot_I: 0.0, 8 | x_dot_D: 0.05, 9 | 10 | y_dot_P: 0.5, 11 | y_dot_I: 0.0, 12 | y_dot_D: 0.05, 13 | 14 | z_dot_P: 0.4, 15 | z_dot_I: 0.25, 16 | z_dot_D: 0.1, 17 | 18 | north_P: 1.0, 19 | north_I: 0.1, 20 | north_D: 0.35, 21 | 22 | east_P: 1.0, 23 | east_I: 0.1, 24 | east_D: 0.2, 25 | 26 | down_P: 1.0, 27 | down_I: 0.0, 28 | down_D: 0.0, 29 | 30 | psi_P: 2.0, 31 | psi_I: 0.0, 32 | psi_D: 0.0, 33 | 34 | tau: 0.05, 35 | 36 | equilibrium_throttle: 0.5, 37 | 38 | max_roll: 0.196, 39 | max_pitch: 0.196, 40 | max_yaw_rate: 0.785, 41 | max_throttle: 0.85, 42 | max_n_dot: 1.5, 43 | max_e_dot: 1.5, 44 | max_d_dot: 0.5, 45 | 46 | # Below this altitude, zero roll, pitch, yawrate will be commanded 47 | min_altitude: 0.5 48 | 49 | } 50 | 51 | estimator: { 52 | ## Post-Process Information 53 | bag_name: "~/roscopter_ws/src/bags/test.bag", 54 | status_topic: "/status", 55 | imu_topic: "/imu/data", 56 | baro_topic: "/serial/baro", 57 | range_topic: "/serial/laser", 58 | pose_topic: "", 59 | odom_topic: "", 60 | gnss_topic: "", 61 | ublox_gnss_topic: "", 62 | inertial_sense_gnss_topic: "/ins/gps", 63 | start_time: 0, 64 | duration: 1000, 65 | ## Is_flying checks 66 | enable_arm_check: false, # require armed before checking for is_flying 67 | is_flying_threshold: 10.5, # if accel measurement exceed this magnitude, set is_flying 68 | update_baro_velocity_threshold: 0.5, 69 | ## Logging 70 | log_prefix: "/tmp/roscopter/ekf/", 71 | enable_log: true, 72 | ## Measurement Processing 73 | enable_partial_update: true, 74 | enable_out_of_order: false, # TODO this isnt fully implemented 75 | use_mocap: false, 76 | use_gnss: true, 77 | use_baro: true, 78 | use_range: false, 79 | use_zero_vel: true, #publishes zeros for the estimator until the quad starts flying, prevents drift before flight 80 | R_zero_vel: [0.001, 0.001, 0.001, # vel 81 | 0.001], # Altitude 82 | ## Initial Uncertainty 83 | P0: [10.0, 10.0, 10.0, # pos 84 | 0.01, 0.01, 0.01, # att 85 | 0.001, 0.001, 0.001, # vel 86 | 0.01, 0.01, 0.01, # accel bias 87 | 0.001, 0.001, 0.001, # gyro bias 88 | 9.0, # baro bias 89 | 100.0], # reference altitude 90 | ## Additive Process Noise 91 | Qx: [0.000, 0.000, 0.000, # pos 92 | 0.000, 0.000, 0.000, # att 93 | 0.00001, 0.00001, 0.00001, # vel 94 | 0.0001, 0.0001, 0.0001, # accel bias 95 | 0.0001, 0.0001, 0.0001, # gyro bias 96 | 0.000001, # baro bias 97 | 1.0], # reference altitude 98 | ## Lambda (Partial Update) 99 | # 1.0 = use full update 100 | # 0.0 = do not update at all 101 | lambda: [1.0, 1.0, 1.0, # pos 102 | 1.0, 1.0, 1.0, # att 103 | 1.0, 1.0, 1.0, # vel 104 | 0.1, 0.1, 0.1, # accel bias 105 | 0.1, 0.1, 0.1, # gyro bias 106 | 0.5, # baro bias 107 | 0.3], # reference altitude 108 | # Constant Parameeters 109 | p_b2g: [0, 0, 0], # position from body frame to gps frame 110 | # Initial State 111 | manual_ref_lla: false, # Use manual ref_lla or use first lla from gps 112 | ref_lla: [40.267320, -111.635629, 1387.0], 113 | ref_heading: 0.0, # reference heading for inertial frame (from north) 114 | x0: [0, 0, 0, # pos 115 | 1, 0, 0, 0], # att 116 | ## Sensor Noise 117 | #imu 118 | accel_noise_stdev: 0.5, 119 | gyro_noise_stdev: 0.02, 120 | #Mocap 121 | position_noise_stdev: 0.01, 122 | attitude_noise_stdev: 0.01, 123 | #barometer 124 | baro_pressure_noise_stdev: 2.0, 125 | #range sensor 126 | range_noise_stdev: 0.1, 127 | #gps 128 | manual_gps_noise: false, # choose noise params for GPS or use the values reported from the message topic 129 | gps_horizontal_stdev: 0.021, 130 | gps_vertical_stdev: 0.04, 131 | gps_speed_stdev: 0.2 132 | } 133 | -------------------------------------------------------------------------------- /roscopter/params/sub_params/controller.yaml: -------------------------------------------------------------------------------- 1 | ### Controller Parameters 2 | 3 | x_dot_P: 0.5 4 | x_dot_I: 0.0 5 | x_dot_D: 0.0 6 | 7 | y_dot_P: 0.5 8 | y_dot_I: 0.0 9 | y_dot_D: 0.0 10 | 11 | z_dot_P: 1.0 12 | z_dot_I: 0.0 13 | z_dot_D: 0.0 14 | 15 | north_P: 0.45 16 | north_I: 0.0 17 | north_d: 0.1 18 | 19 | east_P: 0.45 20 | east_I: 0.0 21 | east_d: 0.1 22 | 23 | down_P: 2.0 24 | down_I: 0.7 25 | down_D: 0.1 26 | 27 | psi_P: 1.0 28 | psi_I: 0.0 29 | psi_D: 0.0 30 | 31 | tau: 0.05 32 | 33 | equilibrium_throttle: 0.5 34 | 35 | max_roll: 0.196 36 | max_pitch: 0.196 37 | max_yaw_rate: 0.785 38 | max_throttle: 0.85 39 | max_n_dot: 1.5 40 | max_e_dot: 1.5 41 | max_d_dot: 1.25 -------------------------------------------------------------------------------- /roscopter/params/sub_params/ekf.yaml: -------------------------------------------------------------------------------- 1 | ## Post-Process Information 2 | bag_name: "~/roscopter_ws/src/bags/test.bag" 3 | status_topic: "/status" 4 | imu_topic: "/imu/data" 5 | baro_topic: "/serial/baro" 6 | range_topic: "/serial/laser" 7 | pose_topic: "" 8 | odom_topic: "" 9 | gnss_topic: "" 10 | ublox_gnss_topic: "" 11 | inertial_sense_gnss_topic: "/ins/gps" 12 | 13 | start_time: 0 14 | duration: 1000 15 | 16 | ## Is_flying checks 17 | enable_arm_check: false # require armed before checking for is_flying 18 | is_flying_threshold: 10.5 # if accel measurement exceed this magnitude, set is_flying 19 | update_baro_velocity_threshold: 0.5 20 | 21 | ## Logging 22 | log_prefix: "/tmp/roscopter/ekf/" 23 | enable_log: true 24 | 25 | ## Measurement Processing 26 | enable_partial_update: true 27 | enable_out_of_order: false # TODO this isnt fully implemented 28 | use_mocap: false 29 | use_gnss: true 30 | use_baro: true 31 | use_range: false 32 | use_zero_vel: true #publishes zeros for the estimator until the quad starts flying, prevents drift before flight 33 | 34 | R_zero_vel: [0.001, 0.001, 0.001, # vel 35 | 0.001] # Altitude 36 | 37 | ## Initial Uncertainty 38 | P0: [10.0, 10.0, 10.0, # pos 39 | 0.01, 0.01, 0.01, # att 40 | 0.001, 0.001, 0.001, # vel 41 | 0.01, 0.01, 0.01, # accel bias 42 | 0.001, 0.001, 0.001, # gyro bias 43 | 9.0, # baro bias 44 | 100.0] # reference altitude 45 | 46 | ## Additive Process Noise 47 | Qx: [0.000, 0.000, 0.000, # pos 48 | 0.000, 0.000, 0.000, # att 49 | 0.00001, 0.00001, 0.00001, # vel 50 | 0.0001, 0.0001, 0.0001, # accel bias 51 | 0.0001, 0.0001, 0.0001, # gyro bias 52 | 0.000001, # baro bias 53 | 1.0] # reference altitude 54 | 55 | ## Lambda (Partial Update) 56 | # 1.0 = use full update 57 | # 0.0 = do not update at all 58 | lambda: [1.0, 1.0, 1.0, # pos 59 | 1.0, 1.0, 1.0, # att 60 | 1.0, 1.0, 1.0, # vel 61 | 0.1, 0.1, 0.1, # accel bias 62 | 0.1, 0.1, 0.1, # gyro bias 63 | 0.5, # baro bias 64 | 0.3] # reference altitude 65 | 66 | # Constant Parameeters 67 | p_b2g: [0, 0, 0] # position from body frame to gps frame 68 | 69 | # Initial State 70 | manual_ref_lla: false # Use manual ref_lla or use first lla from gps 71 | ref_lla: [40.267320, -111.635629, 1387.0] 72 | ref_heading: 0.0 # reference heading for inertial frame (from north) 73 | 74 | x0: [0, 0, 0, # pos 75 | 1, 0, 0, 0] # att 76 | 77 | ## Sensor Noise 78 | accel_noise_stdev: 0.5 79 | gyro_noise_stdev: 0.02 80 | position_noise_stdev: 0.01 # Mocap 81 | attitude_noise_stdev: 0.01 # Mocap 82 | baro_pressure_noise_stdev: 2.0 83 | range_noise_stdev: 0.1 84 | 85 | manual_gps_noise: false # choose noise params for GPS or use the values reported from the message topic 86 | gps_horizontal_stdev: 0.021 87 | gps_vertical_stdev: 0.04 88 | gps_speed_stdev: 0.2 89 | -------------------------------------------------------------------------------- /roscopter/params/sub_params/rosflight_default_parameters.yml: -------------------------------------------------------------------------------- 1 | - {name: ACC_LPF_ALPHA, type: 9, value: 0.5} 2 | - {name: ACC_X_BIAS, type: 9, value: 0.01081923022866249} 3 | - {name: ACC_X_TEMP_COMP, type: 9, value: 0} 4 | - {name: ACC_Y_BIAS, type: 9, value: 0.009584274142980576} 5 | - {name: ACC_Y_TEMP_COMP, type: 9, value: 0} 6 | - {name: ACC_Z_BIAS, type: 9, value: 0.003193239914253354} 7 | - {name: ACC_Z_TEMP_COMP, type: 9, value: 0} 8 | - {name: AIL_REV, type: 6, value: 0} 9 | - {name: ARM_CHANNEL, type: 6, value: 5} 10 | - {name: ARM_SPIN_MOTORS, type: 6, value: 1} 11 | - {name: ARM_THRESHOLD, type: 9, value: 0.1500000059604645} 12 | - {name: BARO_BIAS, type: 9, value: 345.3742370605469} 13 | - {name: BATT_CURR_ALPHA, type: 9, value: 0.9950000047683716} 14 | - {name: BATT_CURR_MULT, type: 9, value: 0} 15 | - {name: BATT_VOLT_ALPHA, type: 9, value: 0.9950000047683716} 16 | - {name: BATT_VOLT_MULT, type: 9, value: 0} 17 | - {name: BAUD_RATE, type: 6, value: 921600} 18 | - {name: CAL_GYRO_ARM, type: 6, value: 0} 19 | - {name: DIFF_PRESS_BIAS, type: 9, value: 0} 20 | - {name: ELEVATOR_REV, type: 6, value: 0} 21 | - {name: FAILSAFE_THR, type: 9, value: 0.300000011920929} 22 | - {name: FC_PITCH, type: 9, value: 0} 23 | - {name: FC_ROLL, type: 9, value: 0} 24 | - {name: FC_YAW, type: 9, value: 0} 25 | - {name: FILTER_ACCMARGIN, type: 9, value: 0.1000000014901161} 26 | - {name: FILTER_INIT_T, type: 6, value: 3000} 27 | - {name: FILTER_KI, type: 9, value: 0.009999999776482582} 28 | - {name: FILTER_KP_ACC, type: 9, value: 0.5} 29 | - {name: FILTER_KP_EXT, type: 9, value: 1.5} 30 | - {name: FILTER_MAT_EXP, type: 6, value: 1} 31 | - {name: FILTER_QUAD_INT, type: 6, value: 1} 32 | - {name: FILTER_USE_ACC, type: 6, value: 1} 33 | - {name: FIXED_WING, type: 6, value: 0} 34 | - {name: GROUND_LEVEL, type: 9, value: 1387} 35 | - {name: GYROXY_LPF_ALPHA, type: 9, value: 0.300000011920929} 36 | - {name: GYROZ_LPF_ALPHA, type: 9, value: 0.300000011920929} 37 | - {name: GYRO_X_BIAS, type: 9, value: -0.0007796076242811978} 38 | - {name: GYRO_Y_BIAS, type: 9, value: 0.004853910766541958} 39 | - {name: GYRO_Z_BIAS, type: 9, value: 0.001492303563281894} 40 | - {name: MAG_A11_COMP, type: 9, value: 1} 41 | - {name: MAG_A12_COMP, type: 9, value: 0} 42 | - {name: MAG_A13_COMP, type: 9, value: 0} 43 | - {name: MAG_A21_COMP, type: 9, value: 0} 44 | - {name: MAG_A22_COMP, type: 9, value: 1} 45 | - {name: MAG_A23_COMP, type: 9, value: 0} 46 | - {name: MAG_A31_COMP, type: 9, value: 0} 47 | - {name: MAG_A32_COMP, type: 9, value: 0} 48 | - {name: MAG_A33_COMP, type: 9, value: 1} 49 | - {name: MAG_X_BIAS, type: 9, value: 0} 50 | - {name: MAG_Y_BIAS, type: 9, value: 0} 51 | - {name: MAG_Z_BIAS, type: 9, value: 0} 52 | - {name: MIN_THROTTLE, type: 6, value: 1} 53 | - {name: MIXER, type: 6, value: 2} 54 | - {name: MOTOR_IDLE_THR, type: 9, value: 0.1000000014901161} 55 | - {name: MOTOR_PWM_UPDATE, type: 6, value: 0} 56 | - {name: OFFBOARD_TIMEOUT, type: 6, value: 100} 57 | - {name: OVRD_LAG_TIME, type: 6, value: 1000} 58 | - {name: PARAM_MAX_CMD, type: 9, value: 1} 59 | - {name: PID_PITCH_ANG_D, type: 9, value: 0.05000000074505806} 60 | - {name: PID_PITCH_ANG_I, type: 9, value: 0} 61 | - {name: PID_PITCH_ANG_P, type: 9, value: 0.1500000059604645} 62 | - {name: PID_PITCH_RATE_D, type: 9, value: 0} 63 | - {name: PID_PITCH_RATE_I, type: 9, value: 0} 64 | - {name: PID_PITCH_RATE_P, type: 9, value: 0.07000000029802322} 65 | - {name: PID_ROLL_ANG_D, type: 9, value: 0.05000000074505806} 66 | - {name: PID_ROLL_ANG_I, type: 9, value: 0} 67 | - {name: PID_ROLL_ANG_P, type: 9, value: 0.1500000059604645} 68 | - {name: PID_ROLL_RATE_D, type: 9, value: 0} 69 | - {name: PID_ROLL_RATE_I, type: 9, value: 0} 70 | - {name: PID_ROLL_RATE_P, type: 9, value: 0.07000000029802322} 71 | - {name: PID_TAU, type: 9, value: 0.05000000074505806} 72 | - {name: PID_YAW_RATE_D, type: 9, value: 0} 73 | - {name: PID_YAW_RATE_I, type: 9, value: 0} 74 | - {name: PID_YAW_RATE_P, type: 9, value: 0.25} 75 | - {name: RC_ATT_CTRL_CHN, type: 6, value: -1} 76 | - {name: RC_ATT_MODE, type: 6, value: 1} 77 | - {name: RC_ATT_OVRD_CHN, type: 6, value: 4} 78 | - {name: RC_F_CHN, type: 6, value: 2} 79 | - {name: RC_MAX_PITCH, type: 9, value: 0.7860000133514404} 80 | - {name: RC_MAX_PITCHRATE, type: 9, value: 3.141590118408203} 81 | - {name: RC_MAX_ROLL, type: 9, value: 0.7860000133514404} 82 | - {name: RC_MAX_ROLLRATE, type: 9, value: 3.141590118408203} 83 | - {name: RC_MAX_YAWRATE, type: 9, value: 1.506999969482422} 84 | - {name: RC_NUM_CHN, type: 6, value: 6} 85 | - {name: RC_OVRD_DEV, type: 9, value: 0.1000000014901161} 86 | - {name: RC_THR_OVRD_CHN, type: 6, value: 4} 87 | - {name: RC_TYPE, type: 6, value: 0} 88 | - {name: RC_X_CHN, type: 6, value: 0} 89 | - {name: RC_Y_CHN, type: 6, value: 1} 90 | - {name: RC_Z_CHN, type: 6, value: 3} 91 | - {name: RUDDER_REV, type: 6, value: 0} 92 | - {name: SERIAL_DEVICE, type: 6, value: 0} 93 | - {name: STRM_AIRSPEED, type: 6, value: 50} 94 | - {name: STRM_ATTITUDE, type: 6, value: 200} 95 | - {name: STRM_BARO, type: 6, value: 50} 96 | - {name: STRM_BATTERY, type: 6, value: 10} 97 | - {name: STRM_GNSS, type: 6, value: 1000} 98 | - {name: STRM_GNSS_FULL, type: 6, value: 10} 99 | - {name: STRM_HRTBT, type: 6, value: 1} 100 | - {name: STRM_IMU, type: 6, value: 250} 101 | - {name: STRM_MAG, type: 6, value: 50} 102 | - {name: STRM_RC, type: 6, value: 50} 103 | - {name: STRM_SERVO, type: 6, value: 50} 104 | - {name: STRM_SONAR, type: 6, value: 40} 105 | - {name: STRM_STATUS, type: 6, value: 10} 106 | - {name: SWITCH_5_DIR, type: 6, value: 1} 107 | - {name: SWITCH_6_DIR, type: 6, value: 1} 108 | - {name: SWITCH_7_DIR, type: 6, value: 1} 109 | - {name: SWITCH_8_DIR, type: 6, value: 1} 110 | - {name: SYS_ID, type: 6, value: 1} 111 | - {name: X_EQ_TORQUE, type: 9, value: 0} 112 | - {name: Y_EQ_TORQUE, type: 9, value: 0} 113 | - {name: Z_EQ_TORQUE, type: 9, value: 0} -------------------------------------------------------------------------------- /roscopter/params/waypoints.yaml: -------------------------------------------------------------------------------- 1 | ### Waypoints ### 2 | 3 | waypoint_manager: { 4 | 5 | waypoints: 6 | [[-2, 3, -4, 0.5], # North, East, Down, Yaw (radians) 7 | [-3, -4, -7, 1.0], 8 | [3, 3, -2, 0.0]], 9 | 10 | threshold: 0.1, 11 | cycle: True 12 | 13 | } -------------------------------------------------------------------------------- /roscopter/scripts/ekf_data.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | 4 | f64 = np.float64 5 | Vec3 = (f64, 3) 6 | Vec4 = (f64, 4) 7 | Vec6 = (f64, 6) 8 | XType = np.dtype([ 9 | ('p', Vec3), 10 | ('q', Vec4) 11 | ]) 12 | 13 | StateType = np.dtype([ 14 | ('t', f64), 15 | ('x', XType), 16 | ('v', Vec3), 17 | ('ba', Vec3), 18 | ('bg', Vec3), 19 | ('bb', f64), 20 | ('ref', f64), 21 | ('a', Vec3), 22 | ('w', Vec3), 23 | ('euler', Vec3) 24 | ]) 25 | 26 | CovType = np.dtype([ 27 | ('t', f64), 28 | ('P', (f64, (17, 17))) 29 | ]) 30 | 31 | RefType = np.dtype([ 32 | ('t', f64), 33 | ('x', XType), 34 | ('euler', Vec3) 35 | ]) 36 | 37 | LlaType = np.dtype([ 38 | ('t', f64), 39 | ('hat', Vec3), 40 | ('bar', Vec3) 41 | ]) 42 | 43 | GnssResType = np.dtype([ 44 | ('t', f64), 45 | ('r', Vec6), 46 | ('z', Vec6), 47 | ('zhat', Vec6) 48 | ]) 49 | 50 | MocapResType = np.dtype([ 51 | ('t', f64), 52 | ('r', Vec6), 53 | ('z', XType), 54 | ('zhat', XType) 55 | ]) 56 | 57 | ZVResType = np.dtype([ 58 | ('t', f64), 59 | ('r', Vec4) 60 | ]) 61 | 62 | ImuType = np.dtype([ 63 | ('t', f64), 64 | ('a', Vec3), 65 | ('w', Vec3) 66 | ]) 67 | 68 | RangeResType = np.dtype([ 69 | ('t', f64), 70 | ('r', f64), 71 | ('z', f64), 72 | ('zhat', f64) 73 | ]) 74 | 75 | BaroResType = np.dtype([ 76 | ('t', f64), 77 | ('r', f64), 78 | ('z', f64), 79 | ('zhat', f64), 80 | ('temp', f64) 81 | ]) 82 | 83 | class Log: 84 | def __init__(self, prefix): 85 | self.prefix = prefix 86 | self.load(prefix) 87 | 88 | def load(self, prefix): 89 | setattr(self, "x", np.fromfile(os.path.join(prefix, "state.bin"), dtype=StateType)) 90 | setattr(self, "cov", np.fromfile(os.path.join(prefix, "cov.bin"), dtype=CovType)) 91 | setattr(self, "gnssRes", np.fromfile(os.path.join(prefix, "gnss_res.bin"), dtype=GnssResType)) 92 | setattr(self, "mocapRes", np.fromfile(os.path.join(prefix, "mocap_res.bin"), dtype=MocapResType)) 93 | setattr(self, "zvRes", np.fromfile(os.path.join(prefix, "zero_vel_res.bin"), dtype=ZVResType)) 94 | setattr(self, "baroRes", np.fromfile(os.path.join(prefix, 95 | "baro_res.bin"), dtype=BaroResType)) 96 | setattr(self, "rangeRes", np.fromfile(os.path.join(prefix, "range_res.bin"), dtype=RangeResType)) 97 | setattr(self, "imu", np.fromfile(os.path.join(prefix, "imu.bin"), dtype=ImuType)) 98 | setattr(self, "lla", np.fromfile(os.path.join(prefix, "lla.bin"), dtype=LlaType)) 99 | setattr(self, "ref", np.fromfile(os.path.join(prefix, "ref.bin"), dtype=RefType)) 100 | -------------------------------------------------------------------------------- /roscopter/scripts/mocap2ublox.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | import numpy as np 4 | 5 | 6 | class Mocap2Ublox(): 7 | 8 | 9 | def __init__(self, Ts, gha, gva, gsa, no, rl, srp, srv, lo, A, B): 10 | 11 | #parameters 12 | self.Ts = Ts 13 | self.global_horizontal_accuracy = gha 14 | self.global_vertical_accuracy = gva 15 | self.global_speed_accuracy = gsa 16 | self.noise_on = no 17 | self.ref_lla = rl 18 | self.sigma_rover_pos = srp 19 | self.sigma_rover_vel = srv 20 | self.lpf_on = lo 21 | self.A = A 22 | self.B = B 23 | self.Kgps = 1100 24 | self.eta_h = 0.03 25 | self.eta_a = 0.06 26 | 27 | #calc ref_ecef from ref_lla 28 | self.F = (self.A - self.B)/self.A # Ellipsoid Flatness 29 | self.E2 = self.F * (2.0 - self.F); # Square of Eccentricity 30 | self.ref_ecef = self.lla2ecef(self.ref_lla) 31 | 32 | #combine standard deviations 33 | self.ned_std_dev_3d = [self.global_horizontal_accuracy, self.global_horizontal_accuracy, self.global_vertical_accuracy] 34 | self.vel_std_dev_3d = [self.global_speed_accuracy, self.global_speed_accuracy, self.global_speed_accuracy] 35 | self.white_noise_3d = [self.eta_h, self.eta_h, self.eta_a] 36 | 37 | #other needed variables and arrays 38 | self.rover_ned = np.zeros(3) 39 | self.rover_ned_prev = np.zeros(3) 40 | self.rover_ned_lpf = np.zeros(3) 41 | self.rover_vel_lpf = np.zeros(3) 42 | self.rover_prev_time = 0.0 43 | self.rover_ned_noise = np.zeros(3) 44 | self.rover_vel_noise = np.zeros(3) 45 | self.rover_vel_noise_prev = np.zeros(3) 46 | 47 | #Outputs 48 | self.rover_virtual_pos_ecef = np.zeros(3) 49 | self.rover_virtual_vel_ecef = np.zeros(3) 50 | 51 | 52 | def update_rover_virtual_PosVelEcef(self, dt): 53 | 54 | #calculate virtual position in ecef frame with noise 55 | self.rover_ned_noise = self.add_gps_noise(self.white_noise_3d, self.rover_ned_noise, self.sigma_rover_pos) 56 | rover_ned_w_noise = self.rover_ned + self.rover_ned_noise 57 | virtual_delta_ecef = self.ned2ecef(rover_ned_w_noise, self.ref_lla) 58 | self.rover_virtual_pos_ecef = self.ref_ecef + virtual_delta_ecef 59 | 60 | #calculate virtual velocity in ecef frame with noise 61 | #make sure we do not divide by zero 62 | if dt != 0.0: 63 | rover_vel = (self.rover_ned - self.rover_ned_prev)/dt 64 | else: 65 | rover_vel = np.zeros(3) 66 | self.rover_vel_noise = self.add_noise_3d(np.zeros(3), self.white_noise_3d) 67 | self.rover_vel_lpf = self.lpf(self.rover_vel_noise, self.rover_vel_noise_prev, self.Ts, self.sigma_rover_vel) 68 | rover_vel_w_noise = rover_vel + self.rover_vel_lpf 69 | self.rover_virtual_vel_ecef = self.ned2ecef(rover_vel_w_noise, self.ref_lla) 70 | 71 | #update histories 72 | self.rover_ned_prev = self.rover_ned 73 | self.rover_vel_prev = rover_vel 74 | self.rover_vel_noise_prev = self.rover_vel_noise 75 | 76 | 77 | def add_noise_3d(self, value, std_dev): 78 | 79 | if self.noise_on: 80 | value_w_noise_1 = np.random.normal(value[0], std_dev[0]) 81 | value_w_noise_2 = np.random.normal(value[1], std_dev[1]) 82 | value_w_noise_3 = np.random.normal(value[2], std_dev[2]) 83 | value_w_noise = np.array([value_w_noise_1, value_w_noise_2, value_w_noise_3]) 84 | return value_w_noise 85 | 86 | else: 87 | return value 88 | 89 | 90 | def add_gps_noise(self, white_noise_3d, noise_prev, sigma): 91 | 92 | #Eq 7.17 Small Unmanned Aircraft (Beard, McLain) pg 139 93 | white_noise = self.add_noise_3d(np.zeros(3), self.white_noise_3d) 94 | color_noise = np.exp(-self.Kgps*self.Ts)*noise_prev 95 | total_noise = white_noise + color_noise 96 | noise = self.lpf(total_noise, noise_prev, self.Ts, sigma) 97 | 98 | return noise 99 | 100 | def lpf(self, xt, x_prev, dt, sigma): 101 | 102 | #low pass filter 103 | if self.lpf_on: 104 | x_lpf = xt*dt/(sigma+dt) + x_prev*sigma/(sigma+dt) 105 | return x_lpf 106 | 107 | else: 108 | return xt 109 | 110 | 111 | def lla2ecef(self, lla): 112 | 113 | lat = lla[0]*np.pi/180 114 | lon = lla[1]*np.pi/180 115 | alt = lla[2] 116 | sinp = np.sin(lat) 117 | cosp = np.cos(lat) 118 | sinl = np.sin(lon) 119 | cosl = np.cos(lon) 120 | e2 = self.E2 121 | v = self.A/np.sqrt(1.0-e2*sinp*sinp) 122 | 123 | ecef = np.zeros(3) 124 | ecef[0]=(v+alt)*cosp*cosl 125 | ecef[1]=(v+alt)*cosp*sinl 126 | ecef[2]=(v*(1.0-e2)+lla[2])*sinp 127 | 128 | return ecef 129 | 130 | 131 | def ned2ecef(self, ned, lla): 132 | 133 | lat = lla[0] 134 | lon = lla[1] 135 | 136 | #don't know why @ isn't working for matrix multiplication 137 | # ecef = Ry(90)Rx(-lon)Ry(lat)ned 138 | ecef = np.matmul(np.matmul(np.matmul(self.Ry(90.0),self.Rx(-lon)),self.Ry(lat)),ned) 139 | 140 | return ecef 141 | 142 | 143 | def Rx(self, theta): 144 | 145 | theta = theta*np.pi/180.0 146 | st = np.sin(theta) 147 | ct = np.cos(theta) 148 | rotx = np.array([[1.0, 0.0, 0.0], \ 149 | [0.0, ct, st], \ 150 | [0.0, -st, ct]]) 151 | 152 | return rotx 153 | 154 | 155 | def Ry(self, theta): 156 | 157 | theta = theta*np.pi/180.0 158 | st = np.sin(theta) 159 | ct = np.cos(theta) 160 | roty = np.array([[ct, 0.0, -st], \ 161 | [0.0, 1.0, 0.0], \ 162 | [st, 0.0, ct]]) 163 | 164 | return roty 165 | 166 | 167 | def Rz(self, theta): 168 | 169 | theta = theta*np.pi/180.0 170 | st = np.sin(theta) 171 | ct = np.cos(theta) 172 | rotz = np.array([[ct, st, 0.0], \ 173 | [-st, ct, 0.0], \ 174 | [0.0, 0.0, 1.0]]) 175 | 176 | return rotz 177 | -------------------------------------------------------------------------------- /roscopter/scripts/mocap2ublox_ros.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import rospy 5 | 6 | from geometry_msgs.msg import PoseStamped 7 | from ublox.msg import PosVelEcef 8 | from rosflight_msgs.msg import GNSS 9 | 10 | from mocap2ublox import Mocap2Ublox 11 | 12 | 13 | class Mocap2UbloxROS(): 14 | 15 | def __init__(self): 16 | 17 | self.load_set_parameters() 18 | 19 | self.m2u = Mocap2Ublox(self.Ts, self.global_horizontal_accuracy, \ 20 | self.global_vertical_accuracy, self.global_speed_accuracy, \ 21 | self.noise_on, self.ref_lla, self.sigma_rover_pos, \ 22 | self.sigma_rover_vel, self.lpf_on, self.A, self.B) 23 | 24 | # Publishers 25 | self.rover_virtual_PosVelEcef_pub_ = rospy.Publisher('rover_PosVelEcef', PosVelEcef, queue_size=5, latch=True) 26 | 27 | # Subscribers 28 | self.rover_mocap_ned_sub_ = rospy.Subscriber('rover_mocap', PoseStamped, self.roverMocapNedCallback, queue_size=5) 29 | 30 | # Timer 31 | self.ublox_rate_timer_ = rospy.Timer(rospy.Duration(self.Ts), self.ubloxRateCallback) 32 | 33 | 34 | while not rospy.is_shutdown(): 35 | # wait for new messages and call the callback when they arrive 36 | rospy.spin() 37 | 38 | 39 | def roverMocapNedCallback(self, msg): 40 | 41 | self.m2u.rover_ned = np.array([msg.pose.position.x, 42 | msg.pose.position.y, 43 | msg.pose.position.z]) 44 | 45 | 46 | def ubloxRateCallback(self, event): 47 | 48 | #publishes all the messages together like ublox would 49 | 50 | #TODO use event to get time 51 | time_stamp = rospy.Time.now() 52 | current_time = time_stamp.secs+time_stamp.nsecs*1e-9 53 | dt = current_time - self.prev_time 54 | self.prev_time = current_time 55 | 56 | #update messages 57 | self.m2u.update_rover_virtual_PosVelEcef(dt) 58 | 59 | #publish messages 60 | self.publish_rover_virtual_PosVelEcef(time_stamp) 61 | 62 | 63 | def publish_rover_virtual_PosVelEcef(self, time_stamp): 64 | 65 | self.rover_PosVelEcef.header.stamp = time_stamp 66 | self.rover_PosVelEcef.fix = 3 67 | # # self.rover_PosVelEcef.lla = self.rover_lla #lla is not currently being used 68 | self.rover_PosVelEcef.position = self.m2u.rover_virtual_pos_ecef 69 | self.rover_PosVelEcef.horizontal_accuracy = self.global_horizontal_accuracy 70 | self.rover_PosVelEcef.vertical_accuracy = self.global_vertical_accuracy 71 | self.rover_PosVelEcef.velocity = self.m2u.rover_virtual_vel_ecef 72 | self.rover_PosVelEcef.speed_accuracy = self.global_speed_accuracy 73 | 74 | self.rover_virtual_PosVelEcef_pub_.publish(self.rover_PosVelEcef) 75 | 76 | 77 | def load_set_parameters(self): 78 | 79 | ublox_frequency = rospy.get_param('~ublox_frequency', 5.0) 80 | self.Ts = 1.0/ublox_frequency 81 | self.global_horizontal_accuracy = rospy.get_param('~global_horizontal_accuracy', 0.4) 82 | self.global_vertical_accuracy = rospy.get_param('~global_vertical_accuracy', 0.6) 83 | self.global_speed_accuracy = rospy.get_param('~global_speed_accuracy', 0.4) 84 | self.noise_on = rospy.get_param('~noise_on', True) 85 | ref_lla = rospy.get_param('~ref_lla', [40.267320, -111.635629, 1387.0]) 86 | self.ref_lla = np.array(ref_lla) 87 | self.sigma_rover_pos = rospy.get_param('~sigma_rover_pos', 5.0) 88 | self.sigma_rover_vel = rospy.get_param('~sigma_rover_vel', 5.0) 89 | self.sigma_rover_relpos = rospy.get_param('~sigma_rover_relpos', 0.0) 90 | self.lpf_on = rospy.get_param('~lpf_on', False) 91 | self.A = rospy.get_param('~A', 6378137.0) 92 | self.B = rospy.get_param('~B', 6356752.314245) 93 | 94 | #message types 95 | self.rover_PosVelEcef = PosVelEcef() 96 | 97 | #used for updating dt 98 | self.prev_time = 0.0 99 | 100 | 101 | if __name__ == '__main__': 102 | rospy.init_node('mocap2ublox_ros', anonymous=True) 103 | try: 104 | mocap2ublox_ros = Mocap2UbloxROS() 105 | except: 106 | rospy.ROSInterruptException 107 | pass -------------------------------------------------------------------------------- /roscopter/scripts/plot_window.py: -------------------------------------------------------------------------------- 1 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 2 | from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar 3 | from mpl_toolkits.mplot3d import Axes3D 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from PyQt5.QtWidgets import QMainWindow, QApplication, QPushButton, QWidget, QAction, QTabWidget,QVBoxLayout 7 | from PyQt5.QtGui import QIcon 8 | from PyQt5.QtCore import pyqtSlot 9 | import sys 10 | import signal 11 | 12 | class PlotWindow(): 13 | def __init__(self, parent=None): 14 | signal.signal(signal.SIGINT, signal.SIG_DFL) 15 | self.app = QApplication(sys.argv) 16 | self.MainWindow = QMainWindow() 17 | self.MainWindow.__init__() 18 | self.MainWindow.setWindowTitle("plot window") 19 | self.canvases = [] 20 | self.figure_handles = [] 21 | self.toolbar_handles = [] 22 | self.tab_handles = [] 23 | self.current_window = -1 24 | self.tabs = QTabWidget() 25 | self.MainWindow.setCentralWidget(self.tabs) 26 | self.MainWindow.resize(1400, 1200) 27 | self.MainWindow.show() 28 | 29 | def addPlot(self, title, figure, threeD=False): 30 | new_tab = QWidget() 31 | layout = QVBoxLayout() 32 | new_tab.setLayout(layout) 33 | 34 | figure.subplots_adjust(left=0.05, right=0.99, bottom=0.05, top=0.91, wspace=0.2, hspace=0.2) 35 | new_canvas = FigureCanvas(figure) 36 | 37 | new_toolbar = NavigationToolbar(new_canvas, new_tab) 38 | 39 | layout.addWidget(new_canvas) 40 | layout.addWidget(new_toolbar) 41 | self.tabs.addTab(new_tab, title) 42 | 43 | self.toolbar_handles.append(new_toolbar) 44 | self.canvases.append(new_canvas) 45 | self.figure_handles.append(figure) 46 | if threeD: 47 | figure.axes[0].mouse_init() 48 | self.tab_handles.append(new_tab) 49 | 50 | def show(self): 51 | self.app.exec_() 52 | 53 | if __name__ == '__main__': 54 | import numpy as np 55 | 56 | 57 | pw = PlotWindow() 58 | 59 | x = np.arange(0, 10, 0.001) 60 | 61 | f = plt.figure() 62 | ysin = np.sin(x) 63 | plt.plot(x, ysin, '--') 64 | pw.addPlot("sin", f) 65 | 66 | f = plt.figure() 67 | ycos = np.cos(x) 68 | plt.plot(x, ycos, '--') 69 | pw.addPlot("cos", f) 70 | pw.show() 71 | 72 | # sys.exit(app.exec_()) 73 | -------------------------------------------------------------------------------- /roscopter/src/controller/controller_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "controller/controller.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "controller_node"); 7 | ros::NodeHandle nh; 8 | 9 | controller::Controller Thing; 10 | 11 | ros::spin(); 12 | 13 | return 0; 14 | } 15 | -------------------------------------------------------------------------------- /roscopter/src/controller/simple_pid.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 Robert Leishman, 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 | #include 33 | 34 | namespace controller 35 | { 36 | // 37 | // Basic initialization 38 | // 39 | SimplePID::SimplePID() 40 | { 41 | kp_ = 0.0; 42 | ki_ = 0.0; 43 | kd_ = 0.0; 44 | integrator_ = 0.0; 45 | differentiator_ = 0.0; 46 | last_error_ = 0.0; 47 | last_state_ = 0.0; 48 | tau_ = 0.0; 49 | max_ = DBL_MAX; 50 | min_ = -DBL_MAX; 51 | } 52 | 53 | // 54 | // Initialize the controller 55 | // 56 | SimplePID::SimplePID(double p, double i, double d, double max, double min, double tau) : 57 | kp_(p), ki_(i), kd_(d), max_(max), min_(min), tau_(tau) 58 | { 59 | integrator_ = 0.0; 60 | differentiator_ = 0.0; 61 | last_error_ = 0.0; 62 | last_state_ = 0.0; 63 | } 64 | 65 | // 66 | // Compute the control; 67 | // 68 | double SimplePID::computePID(double desired, double current, double dt, double x_dot) 69 | { 70 | double error = desired - current; 71 | 72 | // Don't do stupid things (like divide by nearly zero, gigantic control jumps) 73 | if (dt < 0.00001 || std::abs(error) > 9999999) 74 | { 75 | return 0.0; 76 | } 77 | 78 | if (dt > 1.0) 79 | { 80 | // This means that this is a ''stale'' controller and needs to be reset. 81 | // This would happen if we have been operating in a different mode for a while 82 | // and will result in some enormous integrator. 83 | // Or, it means we are disarmed and shouldn't integrate 84 | // Setting dt for this loop will mean that the integrator and dirty derivative 85 | // doesn't do anything this time but will keep it from exploding. 86 | dt = 0.0; 87 | differentiator_ = 0.0; 88 | } 89 | 90 | double p_term = error*kp_; 91 | double i_term = 0.0; 92 | double d_term = 0.0; 93 | 94 | 95 | // Calculate Derivative Term 96 | if (kd_ > 0.0) 97 | { 98 | if (std::isfinite(x_dot)) 99 | { 100 | d_term = kd_ * x_dot; 101 | } 102 | else if (dt > 0.0) 103 | { 104 | // Noise reduction (See "Small Unmanned Aircraft". Chapter 6. Slide 31/33) 105 | // d/dx w.r.t. error:: differentiator_ = (2*tau_ - dt)/(2*tau_ + dt)*differentiator_ + 2/(2*tau_ + dt)*(error - 106 | // last_error_); 107 | differentiator_ = 108 | (2 * tau_ - dt) / (2 * tau_ + dt) * differentiator_ + 2 / (2 * tau_ + dt) * (current - last_state_); 109 | d_term = kd_* differentiator_; 110 | } 111 | } 112 | 113 | // Calculate Integrator Term 114 | if (ki_ > 0.0) 115 | { 116 | integrator_ += dt / 2 * (error + last_error_); // (trapezoidal rule) 117 | i_term = ki_ * integrator_; 118 | } 119 | 120 | // Save off this state for next loop 121 | last_error_ = error; 122 | last_state_ = current; 123 | 124 | // Sum three terms 125 | double u = p_term + i_term - d_term; 126 | 127 | // return u; 128 | 129 | // Integrator anti-windup 130 | double u_sat = saturate(u, min_, max_); 131 | if (u != u_sat && std::fabs(i_term) > fabs(u - p_term + d_term)) 132 | { 133 | // If we are at the saturation limits, then make sure the integrator doesn't get 134 | // bigger if it won't do anything (except take longer to unwind). Just set it to the 135 | // largest value it could be to max out the control 136 | integrator_ = (u_sat - p_term + d_term) / ki_; 137 | } 138 | return u_sat; 139 | } 140 | 141 | 142 | // 143 | // Late initialization or redo 144 | // 145 | void SimplePID::setGains(double p, double i, double d, double tau, double max_u, double min_u) 146 | { 147 | //! \todo Should we really be zeroing this while we are gain tuning? 148 | kp_ = p; 149 | ki_ = i; 150 | kd_ = d; 151 | tau_ = tau; 152 | max_ = max_u; 153 | min_ = min_u; 154 | } 155 | 156 | 157 | } // namespace relative_nav 158 | -------------------------------------------------------------------------------- /roscopter/src/ekf/dynamics.cpp: -------------------------------------------------------------------------------- 1 | #include "ekf/ekf.h" 2 | 3 | using namespace Eigen; 4 | using namespace xform; 5 | using namespace quat; 6 | using namespace std; 7 | 8 | #define T transpose() 9 | 10 | namespace roscopter::ekf 11 | { 12 | 13 | void EKF::dynamics(const State &x, const Vector6d& u, ErrorState &dx, bool calc_jac) 14 | { 15 | Vector3d accel = u.head<3>() - x.ba; 16 | Vector3d omega = u.tail<3>() - x.bg; 17 | 18 | dx.p = x.q.rota(x.v); 19 | dx.q = omega; 20 | dx.v = accel + x.q.rotp(gravity) - omega.cross(x.v); 21 | dx.ba.setZero(); 22 | dx.bg.setZero(); 23 | dx.bb = 0.; 24 | dx.ref = 0.; 25 | 26 | CHECK_NAN(dx.arr); 27 | if (calc_jac) 28 | { 29 | Matrix3d R = x.q.R(); 30 | typedef ErrorState DX; 31 | typedef meas::Imu U; 32 | 33 | A_.setZero(); 34 | B_.setZero(); 35 | A_.block<3,3>(DX::DP, DX::DQ) = -R.T * skew(x.v); 36 | A_.block<3,3>(DX::DP, DX::DV) = R.T; 37 | 38 | A_.block<3,3>(DX::DQ, DX::DQ) = -skew(omega); 39 | A_.block<3,3>(DX::DQ, DX::DBG) = -I_3x3; 40 | B_.block<3,3>(DX::DQ, U::W) = I_3x3; 41 | 42 | A_.block<3,3>(DX::DV, DX::DV) = -skew(omega); 43 | A_.block<3,3>(DX::DV, DX::DQ) = skew(x.q.rotp(gravity)); 44 | A_.block<3,3>(DX::DV, DX::DBA) = -I_3x3; 45 | A_.block<3,3>(DX::DV, DX::DBG) = -skew(x.v); 46 | B_.block<3,3>(DX::DV, U::A) = I_3x3; 47 | B_.block<3,3>(DX::DV, U::W) = skew(x.v); 48 | 49 | CHECK_NAN(A_); CHECK_NAN(B_); 50 | } 51 | } 52 | 53 | void EKF::errorStateDynamics(const State& xhat, const ErrorState& xt, const Vector6d& u, 54 | const Vector6d& eta, ErrorState& dxdot) 55 | { 56 | auto eta_a = eta.head<3>(); 57 | auto eta_w = eta.tail<3>(); 58 | auto z_a = u.head<3>(); 59 | auto z_w = u.tail<3>(); 60 | 61 | State x = xhat + xt; 62 | Vector3d a = z_a - x.ba + eta_a; 63 | Vector3d ahat = z_a - xhat.ba; 64 | Vector3d w = z_w - x.bg + eta_w; 65 | Vector3d what = z_w - xhat.bg; 66 | dxdot.arr.setZero(); 67 | dxdot.p = x.q.rota(x.v) - xhat.q.rota(xhat.v); 68 | dxdot.q = w - x.q.rotp(xhat.q.rota(what)); 69 | dxdot.v = x.q.rotp(gravity) + a - w.cross(x.v) - (xhat.q.rotp(gravity) + ahat - what.cross(xhat.v)); 70 | } 71 | 72 | } 73 | -------------------------------------------------------------------------------- /roscopter/src/ekf/ekf_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ekf/ekf_ros.h" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | ros::init(argc, argv, "estimator"); 7 | 8 | roscopter::ekf::EKF_ROS estimator; 9 | estimator.initROS(); 10 | 11 | ros::spin(); 12 | 13 | return 0; 14 | } 15 | 16 | -------------------------------------------------------------------------------- /roscopter/src/ekf/ekf_rosbag.cpp: -------------------------------------------------------------------------------- 1 | #include "ekf/ekf_rosbag.h" 2 | 3 | using namespace Eigen; 4 | using namespace std; 5 | namespace roscopter::ekf 6 | { 7 | 8 | ROSbagParser::ROSbagParser(int argc, char** argv) 9 | { 10 | start_ = 0; 11 | duration_ = 1e3; 12 | 13 | param_filename_ = ROSCOPTER_DIR"/params/ekf.yaml"; 14 | param_namespace_ = ""; 15 | getArgs(argc, argv); 16 | 17 | loadParams(); 18 | openBag(); 19 | 20 | ekf_.init(param_filename_,param_namespace_); 21 | } 22 | 23 | void ROSbagParser::loadParams() 24 | { 25 | get_yaml_node("bag_name", param_filename_, bag_filename_); 26 | get_yaml_node("status_topic", param_filename_, status_topic_); 27 | get_yaml_node("imu_topic", param_filename_, imu_topic_); 28 | get_yaml_node("baro_topic", param_filename_, baro_topic_); 29 | get_yaml_node("range_topic", param_filename_, range_topic_); 30 | get_yaml_node("pose_topic", param_filename_, pose_topic_); 31 | get_yaml_node("odom_topic", param_filename_, odom_topic_); 32 | get_yaml_node("gnss_topic", param_filename_, gnss_topic_); 33 | get_yaml_node("ublox_gnss_topic", param_filename_, ublox_gnss_topic_); 34 | get_yaml_node("inertial_sense_gnss_topic", param_filename_, inertial_sense_gnss_topic_); 35 | get_yaml_node("start_time", param_filename_, start_); 36 | get_yaml_node("duration", param_filename_, duration_); 37 | } 38 | 39 | void ROSbagParser::displayHelp() 40 | { 41 | std::cout << "ROS bag parser\n"; 42 | std::cout << "-h display this message\n"; 43 | std::cout << "-f configuration yaml file to parse, [" ROSCOPTER_DIR "/params/ekf.yaml]" << std::endl; 44 | exit(0); 45 | } 46 | 47 | void ROSbagParser::getArgs(int argc, char** argv) 48 | { 49 | InputParser argparse(argc, argv); 50 | 51 | if (argparse.cmdOptionExists("-h")) 52 | displayHelp(); 53 | argparse.getCmdOption("-f", param_filename_); 54 | argparse.getCmdOption("-f", param_namespace_); 55 | } 56 | 57 | void ROSbagParser::openBag() 58 | { 59 | try 60 | { 61 | bag_.open(bag_filename_.c_str(), rosbag::bagmode::Read); 62 | view_ = new rosbag::View(bag_); 63 | } 64 | catch(rosbag::BagIOException e) 65 | { 66 | fprintf(stderr, "unable to load rosbag %s, %s", bag_filename_.c_str(), e.what()); 67 | throw e; 68 | } 69 | 70 | bag_start_ = view_->getBeginTime() + ros::Duration(start_); 71 | bag_end_ = bag_start_ + ros::Duration(duration_); 72 | 73 | if (bag_end_ > view_->getEndTime()) 74 | bag_end_ = view_->getEndTime(); 75 | 76 | delete view_; 77 | view_ = new rosbag::View(bag_, bag_start_, bag_end_); 78 | } 79 | 80 | void ROSbagParser::parseBag() 81 | { 82 | ProgressBar prog(view_->size(), 80); 83 | int i = 0; 84 | for(rosbag::MessageInstance const m : (*view_)) 85 | { 86 | if (m.getTime() < bag_start_) 87 | continue; 88 | 89 | if (m.getTime() > bag_end_) 90 | break; 91 | 92 | prog.print(i++, (m.getTime() - bag_start_).toSec()); 93 | 94 | if (m.isType() && m.getTopic().compare(imu_topic_) == 0) 95 | ekf_.imuCallback(m.instantiate()); 96 | else if (m.isType() && m.getTopic().compare(status_topic_) == 0) 97 | ekf_.statusCallback(m.instantiate()); 98 | else if (m.isType() && m.getTopic().compare(baro_topic_) == 0) 99 | ekf_.baroCallback(m.instantiate()); 100 | else if (m.isType() && m.getTopic().compare(range_topic_) == 0) 101 | ekf_.rangeCallback(m.instantiate()); 102 | else if (m.isType() && m.getTopic().compare(pose_topic_) == 0) 103 | ekf_.poseCallback(m.instantiate()); 104 | else if (m.isType() && m.getTopic().compare(odom_topic_) == 0) 105 | ekf_.odomCallback(m.instantiate()); 106 | else if (m.isType() && m.getTopic().compare(gnss_topic_) == 0) 107 | ekf_.gnssCallback(m.instantiate()); 108 | #ifdef UBLOX 109 | else if (m.isType() && m.getTopic().compare(ublox_gnss_topic_) == 0) 110 | ekf_.gnssCallbackUblox(m.instantiate()); 111 | #endif 112 | #ifdef INERTIAL_SENSE 113 | else if (m.isType() && m.getTopic().compare(inertial_sense_gnss_topic_) == 0) 114 | ekf_.gnssCallbackInertialSense(m.instantiate()); 115 | #endif 116 | } 117 | prog.finished(); 118 | cout << endl; 119 | cout.flush(); 120 | } 121 | 122 | 123 | 124 | 125 | } 126 | 127 | 128 | int main(int argc, char** argv) 129 | { 130 | ros::init(argc, argv, "ekf_rosbag_parser"); 131 | roscopter::ekf::ROSbagParser thing(argc, argv); 132 | thing.parseBag(); 133 | } 134 | -------------------------------------------------------------------------------- /roscopter/src/ekf/log.cpp: -------------------------------------------------------------------------------- 1 | #include "ekf/ekf.h" 2 | 3 | #include 4 | 5 | namespace fs = std::experimental::filesystem; 6 | 7 | namespace roscopter::ekf 8 | { 9 | 10 | void EKF::initLog() 11 | { 12 | fs::create_directories(log_prefix_); 13 | for (int i = 0; i < log_names_.size(); i++) 14 | { 15 | logs_[i]->open(fs::path(log_prefix_) / log_names_[i]); 16 | } 17 | } 18 | 19 | 20 | void EKF::logState() 21 | { 22 | logs_[LOG_STATE]->log(x().t); 23 | logs_[LOG_STATE]->logVectors(x().arr); 24 | } 25 | 26 | 27 | void EKF::logCov() 28 | { 29 | logs_[LOG_STATE]->log(x().t); 30 | logs_[LOG_STATE]->log(P()); 31 | } 32 | 33 | 34 | } 35 | 36 | -------------------------------------------------------------------------------- /roscopter/src/ekf/meas.cpp: -------------------------------------------------------------------------------- 1 | #include "ekf/meas.h" 2 | 3 | namespace roscopter::ekf::meas 4 | { 5 | 6 | Base::Base() 7 | { 8 | type = BASE; 9 | handled = false; 10 | } 11 | 12 | std::string Base::Type() const 13 | { 14 | switch (type) 15 | { 16 | case BASE: 17 | return "Base"; 18 | break; 19 | case GNSS: 20 | return "Gnss"; 21 | break; 22 | case IMU: 23 | return "Imu"; 24 | break; 25 | case BARO: 26 | return "Baro"; 27 | break; 28 | case RANGE: 29 | return "Range"; 30 | break; 31 | case MOCAP: 32 | return "Mocap"; 33 | break; 34 | case ZERO_VEL: 35 | return "ZeroVel"; 36 | break; 37 | } 38 | } 39 | 40 | bool basecmp(const Base* a, const Base* b) 41 | { 42 | return a->t < b->t; 43 | } 44 | 45 | 46 | 47 | Imu::Imu(double _t, const Vector6d &_z, const Matrix6d &_R) 48 | { 49 | t = _t; 50 | z = _z; 51 | R = _R; 52 | type = IMU; 53 | } 54 | 55 | Baro::Baro(double _t, const double &_z, const double &_R, const double& _temp) 56 | { 57 | t = _t; 58 | z(0) = _z; 59 | R(0) = _R; 60 | temp = _temp; 61 | type = RANGE; 62 | } 63 | 64 | Range::Range(double _t, const double &_z, const double &_R) 65 | { 66 | t = _t; 67 | z(0) = _z; 68 | R(0) = _R; 69 | type = RANGE; 70 | } 71 | 72 | Gnss::Gnss(double _t, const Vector6d& _z, const Matrix6d& _R) : 73 | p(z.data()), 74 | v(z.data()+3) 75 | { 76 | t = _t; 77 | type = GNSS; 78 | z = _z; 79 | R = _R; 80 | } 81 | 82 | Mocap::Mocap(double _t, const xform::Xformd &_z, const Matrix6d &_R) : 83 | z(_z), 84 | R(_R) 85 | { 86 | t = _t; 87 | type = MOCAP; 88 | } 89 | 90 | ZeroVel::ZeroVel(double _t) 91 | { 92 | t = _t; 93 | type = ZERO_VEL; 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /roscopter/src/ekf/state.cpp: -------------------------------------------------------------------------------- 1 | #include "ekf/state.h" 2 | 3 | using namespace Eigen; 4 | using namespace xform; 5 | using namespace quat; 6 | using namespace std; 7 | 8 | namespace roscopter::ekf 9 | { 10 | 11 | ErrorState::ErrorState() : 12 | x(arr.data()), 13 | p(arr.data()), 14 | q(arr.data()+3), 15 | v(arr.data()+6), 16 | ba(arr.data()+9), 17 | bg(arr.data()+12), 18 | bb(*(arr.data()+15)), 19 | ref(*(arr.data()+16)) 20 | { 21 | arr.setConstant(NAN); 22 | } 23 | 24 | ErrorState::ErrorState(const ErrorState& other) : 25 | ErrorState() 26 | { 27 | arr = other.arr; 28 | } 29 | 30 | ErrorState& ErrorState::operator=(const ErrorState& obj) 31 | { 32 | arr = obj.arr; 33 | return *this; 34 | } 35 | 36 | ErrorState ErrorState::operator* (const double& s) const 37 | { 38 | ErrorState out; 39 | out.arr = s * arr; 40 | return out; 41 | } 42 | 43 | ErrorState ErrorState::operator/ (const double& s) const 44 | { 45 | ErrorState out; 46 | out.arr = arr/s; 47 | return out; 48 | } 49 | 50 | ErrorState& ErrorState::operator*= (const double& s) 51 | { 52 | arr = s * arr; 53 | return *this; 54 | } 55 | 56 | ErrorState ErrorState::operator+ (const ErrorState& obj) const 57 | { 58 | ErrorState out; 59 | out.arr = obj.arr + arr; 60 | return out; 61 | } 62 | 63 | ErrorState ErrorState::operator+ (const Matrix& v) const 64 | { 65 | ErrorState out; 66 | out.arr = v + arr; 67 | return out; 68 | } 69 | 70 | ErrorState& ErrorState::operator+= (const Matrix& v) 71 | { 72 | arr += arr; 73 | return *this; 74 | } 75 | 76 | ErrorState& ErrorState::operator+= (const ErrorState& obj) 77 | { 78 | arr = obj.arr + arr; 79 | return *this; 80 | } 81 | ErrorState ErrorState::operator- (const ErrorState& obj) const 82 | { 83 | ErrorState out; 84 | out.arr = arr - obj.arr; 85 | return out; 86 | } 87 | 88 | ErrorState ErrorState::operator- (const Matrix& v) const 89 | { 90 | ErrorState out; 91 | out.arr = arr - v; 92 | return out; 93 | } 94 | 95 | ErrorState& ErrorState::operator-= (const Matrix& v) 96 | { 97 | arr -= arr; 98 | return *this; 99 | } 100 | 101 | ErrorState& ErrorState::operator-= (const ErrorState& obj) 102 | { 103 | arr = arr - obj.arr; 104 | return *this; 105 | } 106 | 107 | State::State() : 108 | t(*arr.data()), 109 | x(arr.data()+1), 110 | p(arr.data()+1), 111 | q(arr.data()+4), 112 | v(arr.data()+8), 113 | ba(arr.data()+11), 114 | bg(arr.data()+14), 115 | bb(*(arr.data()+17)), 116 | ref(*(arr.data()+18)), 117 | imu(arr.data()+1+NX), 118 | a(arr.data()+1+NX), 119 | w(arr.data()+1+NX+3) 120 | { 121 | //#ifndef NDEBUG 122 | // to help with tracking down uninitialized memory, in debug mode fill with nans 123 | arr.setConstant(NAN); 124 | //#endif 125 | } 126 | 127 | State::State(const State &other) : 128 | State() 129 | { 130 | arr = other.arr; 131 | } 132 | 133 | State& State::operator= (const State& other) 134 | { 135 | arr = other.arr; 136 | return *this; 137 | } 138 | 139 | State State::operator+(const ErrorState& dx) const 140 | { 141 | State xp; 142 | xp.p = p + dx.p; 143 | xp.q = q + dx.q; 144 | xp.v = v + dx.v; 145 | xp.ba = ba + dx.ba; 146 | xp.bg = bg + dx.bg; 147 | xp.bb = bb + dx.bb; 148 | xp.ref = ref + dx.ref; 149 | return xp; 150 | } 151 | 152 | State State::operator+(const Matrix& dx) const 153 | { 154 | State xp; 155 | xp.p = p + dx.segment<3>(ErrorState::DP); 156 | xp.q = q + dx.segment<3>(ErrorState::DQ); 157 | xp.v = v + dx.segment<3>(ErrorState::DV); 158 | xp.ba = ba + dx.segment<3>(ErrorState::DBA); 159 | xp.bg = bg + dx.segment<3>(ErrorState::DBG); 160 | xp.bb = bb + dx(ErrorState::DBB); 161 | xp.ref = ref + dx(ErrorState::DREF); 162 | return xp; 163 | } 164 | 165 | State& State::operator+=(const VectorXd& dx) 166 | { 167 | p += dx.segment<3>(ErrorState::DP); 168 | q += dx.segment<3>(ErrorState::DQ); 169 | v += dx.segment<3>(ErrorState::DV); 170 | ba += dx.segment<3>(ErrorState::DBA); 171 | bg += dx.segment<3>(ErrorState::DBG); 172 | bb += dx(ErrorState::DBB); 173 | ref += dx(ErrorState::DREF); 174 | 175 | *this; 176 | } 177 | 178 | State& State::operator+=(const ErrorState& dx) 179 | { 180 | p = p + dx.p; 181 | q = q + dx.q; 182 | v = v + dx.v; 183 | ba = ba + dx.ba; 184 | bg = bg + dx.bg; 185 | bb = bb + dx.bb; 186 | ref = ref + dx.ref; 187 | 188 | return *this; 189 | } 190 | 191 | ErrorState State:: operator-(const State& dx) const 192 | { 193 | ErrorState del; 194 | del.p = p - dx.p; 195 | del.q = q - dx.q; 196 | del.v = v - dx.v; 197 | del.ba = ba - dx.ba; 198 | del.bg = bg - dx.bg; 199 | del.bb = bb - dx.bb; 200 | del.ref = ref - dx.ref; 201 | return del; 202 | } 203 | 204 | 205 | StateBuf::StateBuf(int _size) : 206 | buf(_size), 207 | size(_size), 208 | head(0), 209 | tail(0) 210 | { 211 | //#ifndef NDEBUG 212 | // to help with tracking down uninitialized memory, in debug mode fill with nans 213 | for (int i = 0; i < buf.size(); i++) 214 | { 215 | buf[i].x.arr.setConstant(NAN); 216 | buf[i].P.setConstant(NAN); 217 | } 218 | //#endif 219 | } 220 | 221 | State& StateBuf::x() 222 | { 223 | return buf[head].x; 224 | } 225 | 226 | const State& StateBuf::x() const 227 | { 228 | return buf[head].x; 229 | } 230 | 231 | dxMat& StateBuf::P() 232 | { 233 | return buf[head].P; 234 | } 235 | 236 | const dxMat& StateBuf::P() const 237 | { 238 | return buf[head].P; 239 | } 240 | 241 | StateBuf::Snapshot& StateBuf::next() 242 | { 243 | return buf[(head + 1) % size]; 244 | } 245 | 246 | void StateBuf::advance() 247 | { 248 | head = (head+1) % size; 249 | if (head == tail) 250 | tail = (tail + 1) % size; 251 | } 252 | 253 | bool StateBuf::rewind(double t) 254 | { 255 | int tmp = head; 256 | while (buf[tmp].x.t > t) 257 | { 258 | tmp = (tmp + size - 1) % size; 259 | if (tmp == head) 260 | return false; 261 | } 262 | head = tmp; 263 | tail = (head + 1) % size; 264 | return true; 265 | } 266 | 267 | StateBuf::Snapshot& StateBuf::begin() 268 | { 269 | return buf[tail]; 270 | } 271 | } 272 | -------------------------------------------------------------------------------- /roscopter/src/waypoint_manager/velocity_commander.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Python controller for landing a multirotor using relative estimates 3 | 4 | import rospy 5 | from math import * 6 | import numpy as np 7 | import time 8 | from rosflight_msgs.msg import Command 9 | from nav_msgs.msg import Odometry 10 | from geometry_msgs.msg import Pose 11 | 12 | class VelocityCommander: 13 | 14 | # Init function 15 | def __init__(self): 16 | 17 | # Init Params here 18 | 19 | self.x_vel = 2.0 20 | self.y_vel = 2.0 21 | self.z_vel = 0.0 # yaw rate 22 | self.alt_command = 10.0 23 | 24 | self.delay_time = 5 25 | 26 | # Init Publishers 27 | self.high_lvl_commands_pub = rospy.Publisher('/leo/velocity_command', Command, queue_size=10) 28 | 29 | # Init the command 30 | self.relative_cmd = Command() 31 | 32 | # begin sending commands 33 | self.sendCommands() 34 | 35 | def sendCommands(self): 36 | 37 | time.sleep(1) 38 | 39 | # send just x command 40 | # pack up the command 41 | print('sending velocity command 1') 42 | self.relative_cmd.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE 43 | self.relative_cmd.x = self.x_vel 44 | self.relative_cmd.y = 0.0 45 | self.relative_cmd.z = self.z_vel 46 | self.relative_cmd.F = -self.alt_command 47 | 48 | self.high_lvl_commands_pub.publish(self.relative_cmd) 49 | 50 | # wait 51 | time.sleep(self.delay_time) 52 | 53 | # send just y command 54 | # pack up the command 55 | print('sending velocity command 2') 56 | self.relative_cmd.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE 57 | self.relative_cmd.x = 0.0 58 | self.relative_cmd.y = self.y_vel 59 | self.relative_cmd.z = self.z_vel 60 | self.relative_cmd.F = -self.alt_command 61 | 62 | self.high_lvl_commands_pub.publish(self.relative_cmd) 63 | 64 | # wait 65 | time.sleep(self.delay_time) 66 | 67 | # send x and y command 68 | # pack up the command 69 | print('sending velocity command 3') 70 | self.relative_cmd.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE 71 | self.relative_cmd.x = -self.x_vel 72 | self.relative_cmd.y = -self.y_vel 73 | self.relative_cmd.z = self.z_vel 74 | self.relative_cmd.F = -self.alt_command 75 | 76 | self.high_lvl_commands_pub.publish(self.relative_cmd) 77 | 78 | # wait 79 | time.sleep(self.delay_time) 80 | 81 | # tell it to stop 82 | # pack up the command 83 | print('sending velocity command 4') 84 | self.relative_cmd.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE 85 | self.relative_cmd.x = 0.0 86 | self.relative_cmd.y = 0.0 87 | self.relative_cmd.z = 0.0 88 | self.relative_cmd.F = -self.alt_command 89 | 90 | self.high_lvl_commands_pub.publish(self.relative_cmd) 91 | 92 | print("done") 93 | 94 | 95 | 96 | ############################## 97 | #### Main Function to Run #### 98 | ############################## 99 | if __name__ == '__main__': 100 | 101 | # Initialize Node 102 | rospy.init_node('velocity_commander') 103 | 104 | # init path_follower object 105 | controller = VelocityCommander() 106 | 107 | rospy.spin() 108 | -------------------------------------------------------------------------------- /roscopter/src/waypoint_manager/velocity_manager.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import numpy as np 4 | 5 | import rospy 6 | 7 | from nav_msgs.msg import Odometry 8 | from rosflight_msgs.msg import Command 9 | from roscopter_msgs.srv import AddWaypoint, RemoveWaypoint, SetWaypointsFromFile 10 | 11 | 12 | class WaypointManager(): 13 | 14 | def __init__(self): 15 | 16 | # get parameters 17 | try: 18 | self.waypoint_list = rospy.get_param('~waypoints') 19 | except KeyError: 20 | rospy.logfatal('waypoints not set') 21 | rospy.signal_shutdown('Parameters not set') 22 | 23 | 24 | # how close does the MAV need to get before going to the next waypoint? 25 | self.threshold = rospy.get_param('~threshold', 5) 26 | self.cyclical_path = rospy.get_param('~cycle', True) 27 | 28 | self.prev_time = rospy.Time.now() 29 | 30 | # set up Services 31 | self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint, self.addWaypointCallback) 32 | self.remove_waypoint_service = rospy.Service('remove_waypoint', RemoveWaypoint, self.addWaypointCallback) 33 | self.set_waypoint_from_file_service = rospy.Service('set_waypoints_from_file', SetWaypointsFromFile, self.addWaypointCallback) 34 | 35 | # Set Up Publishers and Subscribers 36 | self.xhat_sub_ = rospy.Subscriber('/leo/truth_NED', Odometry, self.odometryCallback, queue_size=5) 37 | self.waypoint_pub_ = rospy.Publisher('/leo/velocity_command', Command, queue_size=5, latch=True) 38 | 39 | self.current_waypoint_index = 0 40 | 41 | command_msg = Command() 42 | current_waypoint = np.array(self.waypoint_list[0]) 43 | 44 | command_msg.x = current_waypoint[0] 45 | command_msg.y = current_waypoint[1] 46 | command_msg.F = current_waypoint[2] 47 | if len(current_waypoint) > 3: 48 | command_msg.z = current_waypoint[3] 49 | else: 50 | next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] 51 | delta = next_point - current_waypoint 52 | command_msg.z = np.atan2(delta[1], delta[0]) 53 | command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE 54 | self.waypoint_pub_.publish(command_msg) 55 | 56 | while not rospy.is_shutdown(): 57 | # wait for new messages and call the callback when they arrive 58 | rospy.spin() 59 | 60 | 61 | def addWaypointCallback(req): 62 | print("addwaypoints") 63 | 64 | def removeWaypointCallback(req): 65 | print("remove Waypoints") 66 | 67 | def setWaypointsFromFile(req): 68 | print("set Waypoints from File") 69 | 70 | def saturate(x, max, min): 71 | if x > max: 72 | return max 73 | elif x < min: 74 | return min 75 | return x 76 | 77 | def odometryCallback(self, msg): 78 | # Get error between waypoint and current state 79 | current_waypoint = np.array(self.waypoint_list[self.current_waypoint_index]) 80 | current_position = np.array([msg.pose.pose.position.x, 81 | msg.pose.pose.position.y, 82 | msg.pose.pose.position.z]) 83 | 84 | # orientation in quaternion form 85 | qw = msg.pose.pose.orientation.w 86 | qx = msg.pose.pose.orientation.x 87 | qy = msg.pose.pose.orientation.y 88 | qz = msg.pose.pose.orientation.z 89 | 90 | # yaw from quaternion 91 | y = np.arctan2(2*(qw*qz + qx*qy), 1 - 2*(qy**2 + qz**2)) 92 | 93 | error = current_position - current_waypoint[0:3] 94 | 95 | # replace next line with rotation matrix 96 | # i_to_b = quaternion_matrix([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) 97 | i_to_b = np.eye(3) 98 | print("i_to_b needs to be fixed ---- currently set to be identity", i_to_b) 99 | error_b = i_to_b*error 100 | 101 | if np.linalg.norm(error) < self.threshold: 102 | # Get new waypoint index 103 | self.current_waypoint_index += 1 104 | if self.cyclical_path: 105 | self.current_waypoint_index %= len(self.waypoint_list) 106 | else: 107 | if self.current_waypoint_index > len(self.waypoint_list): 108 | self.current_waypoint_index -=1 109 | next_waypoint = np.array(self.waypoint_list[self.current_waypoint_index]) 110 | command_msg = Command() 111 | command_msg.x = next_waypoint[0] 112 | command_msg.y = next_waypoint[1] 113 | command_msg.F = next_waypoint[2] 114 | if len(current_waypoint) <= 3: 115 | next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] 116 | delta = next_point - current_waypoint 117 | current_waypoint.append(np.atan2(delta[1], delta[0])) 118 | 119 | # altitude send directly 120 | command_msg.F = next_waypoint[2] 121 | 122 | # velocity commands, Proportional gain and saturation 123 | command_msg.x = saturate(2.0*error_b[0], 2.0, -2.0) 124 | command_msg.y = saturate(2.0*error_b[0], 2.0, -2.0) 125 | command_msg.z = saturate(1.507*(current_waypoint[3] - y), 1.507, -1.507) 126 | 127 | command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE 128 | self.waypoint_pub_.publish(command_msg) 129 | 130 | if __name__ == '__main__': 131 | rospy.init_node('waypoint_manager', anonymous=True) 132 | try: 133 | wp_manager = WaypointManager() 134 | except: 135 | rospy.ROSInterruptException 136 | pass 137 | -------------------------------------------------------------------------------- /roscopter_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roscopter_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | message_generation) 7 | 8 | ################################################ 9 | ## Declare ROS messages, services and actions ## 10 | ################################################ 11 | 12 | ## To declare and build messages, services or actions from within this 13 | ## package, follow these steps: 14 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 15 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 16 | ## * In the file package.xml: 17 | ## * add a build_depend tag for "message_generation" 18 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 19 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 20 | ## but can be declared for certainty nonetheless: 21 | ## * add a run_depend tag for "message_runtime" 22 | ## * In this file (CMakeLists.txt): 23 | ## * add "message_generation" and every package in MSG_DEP_SET to 24 | ## find_package(catkin REQUIRED COMPONENTS ...) 25 | ## * add "message_runtime" and every package in MSG_DEP_SET to 26 | ## catkin_package(CATKIN_DEPENDS ...) 27 | ## * uncomment the add_*_files sections below as needed 28 | ## and list every .msg/.srv/.action file to be processed 29 | ## * uncomment the generate_messages entry below 30 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 31 | 32 | ## Generate messages in the 'msg' folder 33 | add_message_files( 34 | FILES 35 | Command.msg 36 | PoseEuler.msg 37 | ) 38 | 39 | # Generate services in the 'srv' folder 40 | add_service_files( 41 | FILES 42 | AddWaypoint.srv 43 | RemoveWaypoint.srv 44 | SetWaypointsFromFile.srv 45 | ListWaypoints.srv 46 | ClearWaypoints.srv 47 | Hold.srv 48 | Release.srv 49 | Land.srv 50 | Fly.srv 51 | ReturnToBase.srv 52 | ) 53 | 54 | ## Generate actions in the 'action' folder 55 | # add_action_files( 56 | # FILES 57 | # Action1.action 58 | # Action2.action 59 | # ) 60 | 61 | ## Generate added messages and services with any dependencies listed here 62 | generate_messages( 63 | DEPENDENCIES 64 | std_msgs # Or other packages containing msgs 65 | ) 66 | 67 | ################################### 68 | ## catkin specific configuration ## 69 | ################################### 70 | ## The catkin_package macro generates cmake config files for your package 71 | ## Declare things to be passed to dependent projects 72 | ## INCLUDE_DIRS: uncomment this if your package contains header files 73 | ## LIBRARIES: libraries you create in this project that dependent projects also need 74 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 75 | ## DEPENDS: system dependencies of this project that dependent projects also need 76 | catkin_package( 77 | # INCLUDE_DIRS include 78 | # LIBRARIES roscopter_msgs 79 | CATKIN_DEPENDS message_runtime 80 | # DEPENDS system_lib 81 | ) 82 | -------------------------------------------------------------------------------- /roscopter_msgs/msg/Command.msg: -------------------------------------------------------------------------------- 1 | # roscopter commands 2 | # can be used for any level of command: 3 | # high level: n_pos, e_pos, d_pos, yaw 4 | # mid level: n_pos, e_pos, d_vel, yaw 5 | # or: n_vel, e_vel, d_pos, yaw_rate 6 | # or: n_vel, e_vel, d_vel, yaw_rate 7 | # low level: n_acc, e_acc, d_acc, yaw_acc 8 | 9 | # uint8 MODE_NORTH_EAST_DOWN_HEADING 10 | uint8 MODE_NPOS_EPOS_DPOS_YAW = 0 11 | uint8 MODE_NVEL_EVEL_DPOS_YAWRATE = 1 12 | uint8 MODE_NACC_EACC_DACC_YAWRATE = 2 13 | uint8 MODE_NVEL_EVEL_DVEL_YAWRATE = 3 14 | uint8 MODE_NPOS_EPOS_DVEL_YAW = 4 15 | 16 | # ros2 17 | # builtin_interfaces/Time stamp 18 | time stamp 19 | 20 | uint8 mode 21 | 22 | float32 cmd1 23 | float32 cmd2 24 | float32 cmd3 25 | float32 cmd4 26 | -------------------------------------------------------------------------------- /roscopter_msgs/msg/PoseEuler.msg: -------------------------------------------------------------------------------- 1 | # Convenience Pose from waypt mgr - Used strictly in NED 2 | float32 n 3 | float32 e 4 | float32 d 5 | # float32 phi 6 | # float32 theta 7 | float32 psi 8 | -------------------------------------------------------------------------------- /roscopter_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roscopter_msgs 4 | 0.0.0 5 | Message, service, and action definitions for the ROScopter package 6 | 7 | Parker Lusk 8 | 9 | TODO 10 | 11 | https://github.com/byu-magicc/roscopter 12 | https://github.com/byu-magicc/roscopter/issues 13 | 14 | catkin 15 | ament_cmake 16 | 17 | 18 | 19 | 20 | message_generation 21 | 22 | message_runtime 23 | message_runtime 24 | 25 | 26 | 27 | 28 | catkin 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/AddWaypoint.srv: -------------------------------------------------------------------------------- 1 | # Add a Waypoint to the waypoint list 2 | float64 x 3 | float64 y 4 | float64 z 5 | float64 psi 6 | int32 index 7 | --- 8 | bool Success 9 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/ClearWaypoints.srv: -------------------------------------------------------------------------------- 1 | # Clears the current waypoint list 2 | 3 | --- 4 | bool Success 5 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/Fly.srv: -------------------------------------------------------------------------------- 1 | # Resumes flying waypoints after landing 2 | 3 | --- 4 | bool Success 5 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/Hold.srv: -------------------------------------------------------------------------------- 1 | # Holds current pose of the multirotor 2 | 3 | --- 4 | bool Success 5 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/Land.srv: -------------------------------------------------------------------------------- 1 | # Has the multirotor land in place 2 | 3 | --- 4 | bool Success 5 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/ListWaypoints.srv: -------------------------------------------------------------------------------- 1 | # Lists the current waypoint list 2 | 3 | --- 4 | bool Success 5 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/Release.srv: -------------------------------------------------------------------------------- 1 | #Releases the Quadrotor from Hold 2 | 3 | --- 4 | bool Success 5 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/RemoveWaypoint.srv: -------------------------------------------------------------------------------- 1 | # Remove a waypoint from the waypoint list 2 | 3 | int32 index # The index of the waypoint to be removed 4 | --- 5 | bool Success 6 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/ReturnToBase.srv: -------------------------------------------------------------------------------- 1 | # Has the multirotor land at [0,0] 2 | 3 | --- 4 | bool Success 5 | -------------------------------------------------------------------------------- /roscopter_msgs/srv/SetWaypointsFromFile.srv: -------------------------------------------------------------------------------- 1 | # Add waypoints from a .txt/.csv file. This will overwrite whatever waypoints 2 | # are currently loaded 3 | 4 | string Filename # The filename of the file to read 5 | --- 6 | bool Success # Whether or not the waypoint list addition was successful 7 | -------------------------------------------------------------------------------- /roscopter_pkgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roscopter_pkgs) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /roscopter_pkgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roscopter_pkgs 4 | 0.1.3 5 | A fully-featured multirotor autopilot for ROS, based on Dr. Beard's Quadrotor Notes. 6 | 7 | Daniel Koch 8 | 9 | Daniel Koch 10 | James Jackson 11 | 12 | BSD 13 | 14 | https://github.com/byu-magicc/roscopter 15 | https://github.com/byu-magicc/roscopter/issues 16 | 17 | catkin 18 | ament_cmake 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | catkin 27 | ament_cmake 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /roscopter_sim/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roscopter_sim) 3 | 4 | find_package(gazebo QUIET) 5 | IF(NOT gazebo_FOUND) 6 | message(STATUS "Gazebo not found. Not building roscopter_sim.") 7 | ELSE() 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 | rosflight_msgs 23 | rosflight_utils 24 | std_msgs 25 | nav_msgs 26 | geometry_msgs 27 | sensor_msgs 28 | std_srvs 29 | gazebo_ros 30 | gazebo_plugins 31 | ) 32 | 33 | find_package(Eigen3 REQUIRED) 34 | link_directories(${GAZEBO_LIBRARY_DIRS}) 35 | 36 | catkin_package( 37 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} 38 | CATKIN_DEPENDS roscpp rosflight_msgs rosflight_utils std_msgs nav_msgs geometry_msgs sensor_msgs std_srvs gazebo_ros gazebo_plugins 39 | DEPENDS EIGEN3 GAZEBO 40 | ) 41 | 42 | include_directories( 43 | include 44 | ${catkin_INCLUDE_DIRS} 45 | ${EIGEN3_INCLUDE_DIRS} 46 | ${GAZEBO_INCLUDE_DIRS} 47 | ) 48 | 49 | add_library(multirotor_forces_and_moments_plugin src/multirotor_forces_and_moments.cpp) 50 | target_link_libraries(multirotor_forces_and_moments_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 51 | add_dependencies(multirotor_forces_and_moments_plugin ${catkin_EXPORTED_TARGETS}) 52 | 53 | install( 54 | TARGETS 55 | multirotor_forces_and_moments_plugin 56 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 57 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | ) 59 | 60 | ENDIF() 61 | -------------------------------------------------------------------------------- /roscopter_sim/include/roscopter_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 ROSCOPTER_SIM_COMMON_H_ 22 | #define ROSCOPTER_SIM_COMMON_H_ 23 | 24 | #include 25 | #include 26 | 27 | namespace gazebo { 28 | 29 | /** 30 | * \brief Obtains a parameter from sdf. 31 | * \param[in] sdf Pointer to the sdf object. 32 | * \param[in] name Name of the parameter. 33 | * \param[out] param Param Variable to write the parameter to. 34 | * \param[in] default_value Default value, if the parameter not available. 35 | * \param[in] verbose If true, gzerror if the parameter is not available. 36 | */ 37 | template 38 | bool getSdfParam(sdf::ElementPtr sdf, const std::string& name, T& param, const T& default_value, const bool& verbose = 39 | false) { 40 | if (sdf->HasElement(name)) { 41 | param = sdf->GetElement(name)->Get(); 42 | return true; 43 | } 44 | else { 45 | param = default_value; 46 | if (verbose) 47 | gzerr << "[roscopter_sim] Please specify a value for parameter \"" << name << "\".\n"; 48 | } 49 | return false; 50 | } 51 | 52 | } // namespace gazebo 53 | 54 | template 55 | class FirstOrderFilter { 56 | /* 57 | This class can be used to apply a first order filter on a signal. 58 | It allows different acceleration and deceleration time constants. 59 | 60 | Short reveiw of discrete time implementation of firest order system: 61 | Laplace: 62 | X(s)/U(s) = 1/(tau*s + 1) 63 | continous time system: 64 | dx(t) = (-1/tau)*x(t) + (1/tau)*u(t) 65 | discretized system (ZoH): 66 | x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) 67 | */ 68 | 69 | public: 70 | FirstOrderFilter(double timeConstantUp, double timeConstantDown, T initialState): 71 | timeConstantUp_(timeConstantUp), 72 | timeConstantDown_(timeConstantDown), 73 | previousState_(initialState) {} 74 | 75 | T updateFilter(T inputState, double samplingTime) { 76 | /* 77 | This method will apply a first order filter on the inputState. 78 | */ 79 | T outputState; 80 | if(inputState > previousState_){ 81 | // Calcuate the outputState if accelerating. 82 | double alphaUp = exp(- samplingTime / timeConstantUp_); 83 | // x(k+1) = Ad*x(k) + Bd*u(k) 84 | outputState = alphaUp * previousState_ + (1 - alphaUp) * inputState; 85 | 86 | }else{ 87 | // Calculate the outputState if decelerating. 88 | double alphaDown = exp(- samplingTime / timeConstantDown_); 89 | outputState = alphaDown * previousState_ + (1 - alphaDown) * inputState; 90 | } 91 | previousState_ = outputState; 92 | return outputState; 93 | 94 | } 95 | ~FirstOrderFilter() {} 96 | 97 | protected: 98 | double timeConstantUp_; 99 | double timeConstantDown_; 100 | T previousState_; 101 | }; 102 | 103 | #endif /* ROSCOPTER_SIM_COMMON_H_ */ 104 | -------------------------------------------------------------------------------- /roscopter_sim/include/roscopter_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 ROSCOPTER_SIM_GZ_COMPAT_H //Include guard 33 | #define ROSCOPTER_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 | -------------------------------------------------------------------------------- /roscopter_sim/include/roscopter_sim/multirotor_forces_and_moments.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 James Jackson, 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 ROSCOPTER_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H 19 | #define ROSCOPTER_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "roscopter_sim/common.h" 41 | #include "roscopter_sim/gz_compat.h" 42 | 43 | namespace gazebo { 44 | 45 | 46 | class MultiRotorForcesAndMoments : public ModelPlugin { 47 | public: 48 | MultiRotorForcesAndMoments(); 49 | 50 | ~MultiRotorForcesAndMoments(); 51 | 52 | void InitializeParams(); 53 | void SendForces(); 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_topic_; 64 | std::string attitude_topic_; 65 | std::string joint_name_; 66 | std::string link_name_; 67 | std::string parent_frame_id_; 68 | std::string motor_speed_pub_topic_; 69 | std::string namespace_; 70 | 71 | physics::WorldPtr world_; 72 | physics::ModelPtr model_; 73 | physics::LinkPtr link_; 74 | physics::JointPtr joint_; 75 | physics::EntityPtr parent_link_; 76 | event::ConnectionPtr updateConnection_; // Pointer to the update event connection. 77 | 78 | // physical parameters 79 | double linear_mu_; 80 | double angular_mu_; 81 | struct GE_constants{ 82 | double a; 83 | double b; 84 | double c; 85 | double d; 86 | double e; 87 | } ground_effect_; 88 | double mass_; // for static thrust offset when in altitude mode (kg) 89 | 90 | // Container for an Actuator 91 | struct Actuator{ 92 | double max; 93 | double tau_up; 94 | double tau_down; 95 | }; 96 | 97 | // Struct of Actuators 98 | // This organizes the physical limitations of the abstract torques and Force 99 | struct Actuators{ 100 | Actuator l; 101 | Actuator m; 102 | Actuator n; 103 | Actuator F; 104 | } actuators_; 105 | 106 | // container for forces 107 | struct ForcesAndTorques{ 108 | double Fx; 109 | double Fy; 110 | double Fz; 111 | double l; 112 | double m; 113 | double n; 114 | } applied_forces_, actual_forces_, desired_forces_; 115 | 116 | // container for PID controller 117 | rosflight_utils::SimplePID roll_controller_; 118 | rosflight_utils::SimplePID pitch_controller_; 119 | rosflight_utils::SimplePID yaw_controller_; 120 | rosflight_utils::SimplePID alt_controller_; 121 | 122 | rosflight_msgs::Command command_; 123 | 124 | // Time Counters 125 | double sampling_time_; 126 | double prev_sim_time_; 127 | bool cmd_valid_; 128 | 129 | ros::NodeHandle* nh_; 130 | ros::NodeHandle nh_private_; 131 | ros::Subscriber command_sub_; 132 | ros::Subscriber wind_sub_; 133 | ros::Publisher attitude_pub_; 134 | 135 | boost::thread callback_queue_thread_; 136 | void QueueThread(); 137 | void WindCallback(const geometry_msgs::Vector3& wind); 138 | void CommandCallback(const rosflight_msgs::Command msg); 139 | void ComputeControl(void); 140 | double sat(double x, double max, double min); 141 | double max(double x, double y); 142 | 143 | std::unique_ptr> rotor_velocity_filter_; 144 | GazeboVector W_wind_; 145 | }; 146 | } 147 | 148 | #endif // ROSCOPTER_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H 149 | -------------------------------------------------------------------------------- /roscopter_sim/launch/gazebo.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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /roscopter_sim/launch/holodeck.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /roscopter_sim/launch/sub_launch/spawn_mav_gazebo.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 | 50 | 51 | 52 | 53 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /roscopter_sim/launch/sub_launch/spawn_mav_holodeck.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 | -------------------------------------------------------------------------------- /roscopter_sim/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roscopter_sim 4 | 2.0.1 5 | The roscopter_sim package 6 | 7 | James Jackson 8 | 9 | James Jackson 10 | Gary Ellingson 11 | Fadri Furrer 12 | Michael Burri 13 | Mina Kamel 14 | Janosch Nikolic 15 | Markus Achtelik 16 | 17 | ASL 2.0 18 | 19 | catkin 20 | ament_cmake 21 | 22 | 23 | 24 | 25 | 26 | cmake_modules 27 | gazebo 28 | 29 | 30 | gazebo_ros 31 | gazebo_plugins 32 | geometry_msgs 33 | roscpp 34 | std_srvs 35 | rosflight_msgs 36 | rosflight_utils 37 | nav_msgs 38 | std_msgs 39 | sensor_msgs 40 | 41 | 42 | 43 | 44 | 45 | catkin 46 | ament_cmake 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /roscopter_sim/params/gazebo/gazebo_world.yaml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/roscopter_sim/params/gazebo/gazebo_world.yaml -------------------------------------------------------------------------------- /roscopter_sim/params/gazebo/multirotor_gazebo_roscopter_sim.yaml: -------------------------------------------------------------------------------- 1 | 2 | ################## Commonly Used Parameters ################### 3 | #common parameters used in varying simulations, create as anchor nodes to reference below 4 | common: { 5 | #dynamics 6 | mass: &mass 3.69 7 | } 8 | 9 | ############# Gazebo Properties ################### 10 | 11 | gazebo_properties: { 12 | mass: *mass, 13 | body_width: 0.505, 14 | body_height: 0.1, 15 | ixx: 0.07, 16 | ixy: 0.0, 17 | ixz: 0.0, 18 | iyy: 0.08, 19 | iyz: 0.0, 20 | izz: 0.12, 21 | } 22 | 23 | ############ ROScopter SIL Parameters #################### 24 | 25 | roscopter_sim: { 26 | # Simplified Dynamics # 27 | dynamics: { 28 | mass: *mass, 29 | linear_mu: 0.2, 30 | angular_mu: 0.3, 31 | ground_effect: [-55.3516, 181.8265, -203.9874, 85.3735, -7.6619], 32 | max_l: 6.5080, 33 | max_m: 5.087, 34 | max_n: 0.25, 35 | max_F: 59.844, 36 | tau_up_l: 0.1904, 37 | tau_up_m: 0.1904, 38 | tau_up_n: 0.1644, 39 | tau_up_F: 0.1644, 40 | tau_down_l: 0.1904, 41 | tau_down_m: 0.1904, 42 | tau_down_n: 0.2164, 43 | tau_down_F: 0.2164, 44 | roll_P: 25.0, 45 | roll_I: 0.0, 46 | roll_D: 8.0, 47 | pitch_P: 25.0, 48 | pitch_I: 0.0, 49 | pitch_D: 8.0, 50 | yaw_P: 25.0, 51 | yaw_I: 0.0, 52 | yaw_D: 0.0, 53 | alt_P: 16.0, 54 | alt_I: 5.0, 55 | alt_D: 32.0 56 | } 57 | } 58 | 59 | ################## rosflight_plugins - Sensor Model Parameters #################### 60 | 61 | rosflight_plugins: { 62 | # Odometry (Ground Truth) 63 | odometry: { 64 | transform_topic: ground_truth/transform, 65 | odometry_topic: ground_truth/odometry 66 | }, 67 | 68 | # IMU 69 | imu: { 70 | noise_on: true, 71 | topic: imu/data, 72 | rate: 250, 73 | gyro_bias_topic: imu/gyro_bias, 74 | gyro_stdev: 0.005 , 75 | gyro_bias_range: 0.005, 76 | gyro_bias_walk_stdev: 0.00001, 77 | acc_bias_topic: imu/acc_bias, 78 | acc_stdev: 0.05, 79 | acc_bias_range: 0.01, 80 | acc_bias_walk_stdev: 0.00001 81 | }, 82 | 83 | # Barometer 84 | barometer: { 85 | noise_on: true, 86 | topic: baro/data, 87 | rate: 50.0, 88 | stdev: 0.1 89 | }, 90 | 91 | # Magnetometer 92 | magnetometer: { 93 | noise_on: true, 94 | rate: 100.0, 95 | topic: mag/data, 96 | stdev: 0.02, 97 | bias_range: 0.15, 98 | declination: 0.198584539676, 99 | inclination: 1.14316156541 100 | }, 101 | 102 | # Sonar 103 | # this is still implemented the old way, we need to write a custom plugin to use 104 | # the ROS parameter server like the others 105 | 106 | # GPS 107 | gps: { 108 | noise_on: true, 109 | rate: 10.0, 110 | topic: gps/data, 111 | north_stdev: 0.21, 112 | east_stdev: 0.21, 113 | alt_stdev: 0.40, 114 | velocity_stdev: 0.1, 115 | k_north: 0.0009090909, # 1/1100 116 | k_east: 0.0009090909, # 1/1100 117 | k_alt: 0.0009090909, # 1/1100 118 | initial_latitude: 40.267320, 119 | initial_longitude: -111.635629, 120 | initial_altitude: 1387.0, 121 | num_sats: 7 122 | } 123 | } -------------------------------------------------------------------------------- /roscopter_sim/params/gazebo/multirotor_gazebo_rosflight_sim.yaml: -------------------------------------------------------------------------------- 1 | 2 | ################## Commonly Used Parameters ################### 3 | #common parameters used in varying simulations, create as anchor nodes to reference below 4 | 5 | common: { 6 | #dynamics 7 | mass: &mass 3.69, 8 | #IMU 9 | gyro_stdev: &gyro_stdev 0.005, 10 | gyro_bias_range: &gyro_bias_range 0.005, 11 | gyro_bias_walk_stdev: &gyro_bias_walk_stdev 0.00001, 12 | acc_stdev: &acc_stdev 0.05, 13 | acc_bias_range: &acc_bias_range 0.01, 14 | acc_bias_walk_stdev: &acc_bias_walk_stdev 0.00001, 15 | #Barometer 16 | baro_stdev: &baro_stdev 0.1, 17 | #Magnetometer 18 | mag_stdev: &mag_stdev 0.02, 19 | mag_bias_range: &mag_bias_range 0.15, 20 | inclination: &inclination 1.14316156541, 21 | declination: &declination 0.198584539676, 22 | } 23 | 24 | ############# Gazebo Properties ################### 25 | 26 | gazebo_properties: { 27 | mass: *mass, 28 | body_width: 0.505, 29 | body_height: 0.1, 30 | ixx: 0.07, 31 | ixy: 0.0, 32 | ixz: 0.0, 33 | iyy: 0.08, 34 | iyz: 0.0, 35 | izz: 0.12, 36 | } 37 | 38 | ################## rosflight_plugins - Sensor Model Parameters #################### 39 | 40 | rosflight_plugins: { 41 | # Odometry (Ground Truth) 42 | odometry: { 43 | transform_topic: ground_truth/transform, 44 | odometry_topic: ground_truth/odometry 45 | }, 46 | 47 | # IMU 48 | imu: { 49 | noise_on: true, 50 | topic: imu/data, 51 | rate: 250, 52 | gyro_bias_topic: imu/gyro_bias, 53 | gyro_stdev: *gyro_stdev , 54 | gyro_bias_range: *gyro_bias_range, 55 | gyro_bias_walk_stdev: *gyro_bias_walk_stdev, 56 | acc_bias_topic: imu/acc_bias, 57 | acc_stdev: *acc_stdev, 58 | acc_bias_range: *acc_bias_range, 59 | acc_bias_walk_stdev: *acc_bias_walk_stdev 60 | }, 61 | 62 | # Barometer 63 | barometer: { 64 | noise_on: true, 65 | topic: baro/data, 66 | rate: 50.0, 67 | stdev: *baro_stdev 68 | }, 69 | 70 | # Magnetometer 71 | magnetometer: { 72 | noise_on: true, 73 | rate: 100.0, 74 | topic: mag/data, 75 | stdev: *mag_stdev, 76 | bias_range: *mag_bias_range, 77 | declination: *declination, 78 | inclination: *inclination 79 | }, 80 | 81 | # Sonar 82 | # this is still implemented the old way, we need to write a custom plugin to use 83 | # the ROS parameter server like the others 84 | 85 | # GPS 86 | gps: { 87 | noise_on: true, 88 | rate: 10.0, 89 | topic: gps/data, 90 | north_stdev: 0.21, 91 | east_stdev: 0.21, 92 | alt_stdev: 0.40, 93 | velocity_stdev: 0.1, 94 | k_north: 0.0009090909, # 1/1100 95 | k_east: 0.0009090909, # 1/1100 96 | k_alt: 0.0009090909, # 1/1100 97 | initial_latitude: 40.267320, 98 | initial_longitude: -111.635629, 99 | initial_altitude: 1387.0, 100 | num_sats: 7 101 | } 102 | } 103 | 104 | ############### ROSflight SIL Parameters ###################### 105 | rosflight_sim: { 106 | mass: *mass, 107 | linear_mu: 0.2, 108 | angular_mu: 0.3, 109 | ground_effect: [-55.3516, 181.8265, -203.9874, 85.3735, -7.6619], 110 | # Dynamics 111 | num_rotors: 4, 112 | rotor_positions: [ 0.1926, 0.230, -0.0762, 113 | -0.1907, 0.205, -0.0762, 114 | -0.1907, -0.205, -0.0762, 115 | 0.1926, -0.230, -0.0762], 116 | rotor_vector_normal: [-0.02674078, 0.0223925, -0.99939157, 117 | 0.02553726, 0.02375588, -0.99939157, 118 | 0.02553726, -0.02375588, -0.99939157, 119 | -0.02674078, -0.0223925, -0.99939157], 120 | rotor_rotation_directions: [-1, 1, -1, 1], 121 | rotor_max_thrust: 14.961, 122 | rotor_F: [1.5e-5, -0.024451, 9.00225], 123 | rotor_T: [2.22e-7,-3.51e-4, 0.12531], 124 | rotor_tau_up: 0.2164, 125 | rotor_tau_down: 0.1644, 126 | ground_altitude: 1387, #Default in Provo, UT (4551 ft) 127 | #IMU 128 | gyro_stdev: *gyro_stdev , 129 | gyro_bias_range: *gyro_bias_range, 130 | gyro_bias_walk_stdev: *gyro_bias_walk_stdev, 131 | acc_stdev: *acc_stdev, 132 | acc_bias_range: *acc_bias_range, 133 | acc_bias_walk_stdev: *acc_bias_walk_stdev, 134 | #Barometer 135 | baro_stdev: *baro_stdev, 136 | baro_bias_range: 500, 137 | baro_bias_walk_stdev: 0.1, 138 | #Sonar 139 | sonar_stdev: 0.03, 140 | sonar_min_range: 0.25, 141 | sonar_max_range: 8.0, 142 | #Pitot 143 | airspeed_stdev: 1.15, 144 | airspeed_bias_range: 0.15, 145 | airspeed_bias_walk_stdev: 0.001, 146 | #Magnetometer 147 | mag_stdev: *mag_stdev, 148 | mag_bias_range: *mag_bias_range, 149 | mag_bias_walk_stdev: 0.001, 150 | inclination: *inclination, 151 | declination: *declination, 152 | } 153 | -------------------------------------------------------------------------------- /roscopter_sim/params/holodeck/holodeck_world.yaml: -------------------------------------------------------------------------------- 1 | # ROSPARAMS FOR ROSFLIGHT_HOLODECK SIMULATOR 2 | 3 | ### rosflight_holodeck ### 4 | world_name: "UrbanCity-Default" 5 | # world_name: "Ocean-BoatLanding" 6 | 7 | # Set the initial state of main UAV agent 8 | # NOTE: - NED inertial frame, i.e. -20 in z means 20 meters above ground 9 | # - Attitude is in DEGREES and EULER ANGLES in this file for convenience, but 10 | # is converted to RADIANS and QUATERNIONS in code 11 | x0: [ 12 | 0, 0, 0, # position NED [0-2] 13 | 0, 0, 0, # attitude [3-6] -- set here as euler angs (degs); will be converted to quat:[w,x,y,z] 14 | 0, 0, 0, # velocity [7-9] 15 | 0, 0, 0, # omega [10-12] 16 | 0, 0, 0 # acc [13-15] 17 | ] 18 | show_viewport: true 19 | framerate: 30 ####### TODO: Currently not working ####### frame rate of camera/rendering [Hz] 20 | simrate: 1000 # rate of simulated dynamics 21 | pub_external_att: false # publish /truth/NED quaternion to /external_attitude topic 22 | 23 | show_camera: false # opencv window of onboard camera sensor 24 | camera_angle: [0, 0, 0] # roll, pitch, yaw of camera in body frame (0,0,0 is pointing forward) [degrees] 25 | 26 | render_quality: 3 # 0 = lowest, 3 = best [int] 27 | day_time: 13 # time of day (24-hour clock, i.e. 18 = 6pm) [int] 28 | weather: "sunny" # can be 'sunny', 'cloudy', or 'rain' 29 | fog_density: 0.0 # value between 0.0 and 1.0 [float] 30 | day_length: 0 # minutes for 1 simulated day/night cycle (0 for no sun movement) [int] 31 | 32 | # Params specific to Ocean world 33 | ocean: { 34 | ocean_state: [1, 1, 90], # [wave intensity (1-13), wave size (1-8), wave direction (0-360)] 35 | boat_initial_state: [ 36 | 20, 0, 0, # position [NED] 37 | 0 # attitude [yaw angle] 38 | ], 39 | boat_cmd: [0, -50], # [direction (0-360), throttle (0-[-100] -- needs to be negative for forward propulsion for some reason)] 40 | enable_aruco: true, 41 | ground_height: -0.1 # meters 42 | } 43 | 44 | ### rosflight_joy ### 45 | rc_node: { 46 | auto_arm: true, 47 | auto_takeoff: true # ignored if 'auto_arm' is set to false 48 | } 49 | -------------------------------------------------------------------------------- /roscopter_sim/params/holodeck/multirotor_holodeck.yaml: -------------------------------------------------------------------------------- 1 | # Parameters for ROSflight software-in-the-loop simulation 2 | mav_type: "multirotor" 3 | 4 | # Common Global Physical Parameters 5 | mass: 2.856 6 | linear_mu: 0.2 7 | angular_mu: 0.3 8 | ground_effect: [-55.3516, 181.8265, -203.9874, 85.3735, -7.6619] 9 | 10 | # Dynamics 11 | num_rotors: 4 12 | rotor_positions: [ 0.1926, 0.230, -0.0762, 13 | -0.1907, 0.205, -0.0762, 14 | -0.1907, -0.205, -0.0762, 15 | 0.1926, -0.230, -0.0762] 16 | # rotor_positions: [ 0.1591, 0.1591, 0.0, 17 | # -0.1591, 0.1591, 0.0, 18 | # -0.1591,-0.1591, 0.0, 19 | # 0.1591,-0.1591, 0.0] 20 | 21 | rotor_vector_normal: [-0.02674078, 0.0223925, -0.99939157, 22 | 0.02553726, 0.02375588, -0.99939157, 23 | 0.02553726, -0.02375588, -0.99939157, 24 | -0.02674078, -0.0223925, -0.99939157] 25 | # rotor_vector_normal: [0.0, 0.0, -1.0, 26 | # 0.0, 0.0, -1.0, 27 | # 0.0, 0.0, -1.0, 28 | # 0.0, 0.0, -1.0] 29 | 30 | rotor_rotation_directions: [-1, 1, -1, 1] 31 | rotor_max_thrust: 14.961 32 | rotor_F: [1.5e-5, -0.024451, 9.00225] 33 | rotor_T: [2.22e-7,-3.51e-4, 0.12531] 34 | rotor_tau_up: 0.2164 35 | rotor_tau_down: 0.1644 36 | 37 | inertia: [0.07, 0.0, 0.0, 38 | 0.0, 0.08, 0.0, 39 | 0.0, 0.0, 0.12] 40 | 41 | 42 | ground_altitude: 1387 #Default in Provo, UT (4551 ft) 43 | 44 | imu_update_rate: 1000.0 45 | 46 | # Sensor Noise Parameters (These are empirically-determined) 47 | # gyro_stdev: 0.00025 48 | # gyro_bias_range: 0.00025 49 | # gyro_bias_walk_stdev: 0.00001 50 | 51 | # acc_stdev: 0.00561 52 | # acc_bias_range: 0.006 53 | # acc_bias_walk_stdev: 0.00001 54 | 55 | gyro_stdev: 0.0001 56 | gyro_bias_range: 0.0 57 | gyro_bias_walk_stdev: 0.0 58 | 59 | acc_stdev: 0.0001 60 | acc_bias_range: 0.0 61 | acc_bias_walk_stdev: 0.0 62 | 63 | baro_stdev: 1.0 64 | baro_bias_range: 10 65 | baro_bias_walk_stdev: 0.1 66 | 67 | sonar_stdev: 0.03 68 | sonar_min_range: 0.25 69 | sonar_max_range: 8.0 70 | 71 | mag_stdev: 1.15 72 | mag_bias_range: 0.15 73 | mag_bias_walk_stdev: 0.001 74 | 75 | inclination: 1.14316156541 76 | declination: 0.198584539676 77 | -------------------------------------------------------------------------------- /roscopter_sim/xacro/multirotor_forces_and_moments.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 8 | 13 | 14 | 15 | 16 | 17 | 24 | 25 | 26 | 27 | 28 | ${namespace} 29 | ${parent_link} 30 | ${wind_topic} 31 | ${command_topic} 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /roscopter_sim/xacro/roscopter_sim.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | Gazebo/${color} 65 | 66 | 67 | 68 | 69 | 70 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | -------------------------------------------------------------------------------- /roscopter_sim/xacro/rosflight_sim.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | Gazebo/${color} 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | ${mav_namespace}/base_link 73 | $(arg rosflight_sim_ns) 74 | multirotor 75 | ${mav_namespace} 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | -------------------------------------------------------------------------------- /roscopter_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roscopter_utils) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | roscpp 7 | ) 8 | catkin_package( 9 | INCLUDE_DIRS include lib/geometry/include 10 | ) 11 | 12 | include_directories(lib/geometry/include include) 13 | -------------------------------------------------------------------------------- /roscopter_utils/include/roscopter_utils/gnss.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "geometry/xform.h" 5 | 6 | namespace roscopter 7 | { 8 | 9 | typedef std::vector> VecVec3; 10 | static constexpr double A = 6378137.0; // WGS-84 Earth semimajor axis (m) 11 | static constexpr double B = 6356752.314245; // Derived Earth semiminor axis (m) 12 | static constexpr double F = (A - B) / A; // Ellipsoid Flatness 13 | static constexpr double F_INV = 1.0 / F; // Inverse flattening 14 | static constexpr double A2 = A * A; 15 | static constexpr double B2 = B * B; 16 | static constexpr double E2 = F * (2 - F); // Square of Eccentricity 17 | 18 | static void ecef2lla(const Eigen::Vector3d& ecef, Eigen::Vector3d& lla) 19 | { 20 | static const double e2 = F * (2.0 - F); 21 | 22 | double r2 = ecef.x()*ecef.x() + ecef.y()*ecef.y(); 23 | double z=ecef.z(); 24 | double v; 25 | double zk; 26 | do 27 | { 28 | zk = z; 29 | double sinp = z / std::sqrt(r2 + z*z); 30 | v = A / std::sqrt(1.0 - e2*sinp*sinp); 31 | z = ecef.z() + v*e2*sinp; 32 | } 33 | while (std::abs(z - zk) >= 1e-4); 34 | 35 | lla.x() = r2 > 1e-12 ? std::atan(z / std::sqrt(r2)) : (ecef.z() > 0.0 ? M_PI/2.0 : -M_PI/2.0); 36 | lla.y() = r2 > 1e-12 ? std::atan2(ecef.y(), ecef.x()) : 0.0; 37 | lla.z() = std::sqrt(r2+z*z) - v; 38 | } 39 | 40 | static Eigen::Vector3d ecef2lla(const Eigen::Vector3d& ecef) 41 | { 42 | Eigen::Vector3d lla; 43 | ecef2lla(ecef, lla); 44 | return lla; 45 | } 46 | 47 | static void lla2ecef(const Eigen::Vector3d& lla, Eigen::Vector3d& ecef) 48 | { 49 | double sinp=sin(lla[0]); 50 | double cosp=cos(lla[0]); 51 | double sinl=sin(lla[1]); 52 | double cosl=cos(lla[1]); 53 | double e2=F*(2.0-F); 54 | double v=A/sqrt(1.0-e2*sinp*sinp); 55 | 56 | ecef[0]=(v+lla[2])*cosp*cosl; 57 | ecef[1]=(v+lla[2])*cosp*sinl; 58 | ecef[2]=(v*(1.0-e2)+lla[2])*sinp; 59 | } 60 | 61 | static Eigen::Vector3d lla2ecef(const Eigen::Vector3d& lla) 62 | { 63 | Eigen::Vector3d ecef; 64 | lla2ecef(lla, ecef); 65 | return ecef; 66 | } 67 | 68 | static quat::Quatd q_e2n(const Eigen::Vector3d& lla) 69 | { 70 | quat::Quatd q1, q2; 71 | q1 = quat::Quatd::from_axis_angle(e_z, lla(1)); 72 | q2 = quat::Quatd::from_axis_angle(e_y, -M_PI/2.0 - lla(0)); 73 | return q1 * q2; 74 | } 75 | 76 | static void x_ecef2ned(const Eigen::Vector3d& ecef, xform::Xformd& X_e2n) 77 | { 78 | X_e2n.q() = q_e2n(ecef2lla(ecef)); 79 | X_e2n.t() = ecef; 80 | } 81 | 82 | static xform::Xformd x_ecef2ned(const Eigen::Vector3d& ecef) 83 | { 84 | xform::Xformd X_e2n; 85 | x_ecef2ned(ecef, X_e2n); 86 | return X_e2n; 87 | } 88 | 89 | static Eigen::Vector3d ned2ecef(const xform::Xformd x_e2n, const Eigen::Vector3d& ned) 90 | { 91 | return x_e2n.transforma(ned); 92 | } 93 | 94 | static void ned2ecef(const xform::Xformd x_e2n, const Eigen::Vector3d& ned, Eigen::Vector3d& ecef) 95 | { 96 | ecef = x_e2n.transforma(ned); 97 | } 98 | 99 | static Eigen::Vector3d ecef2ned(const xform::Xformd x_e2n, const Eigen::Vector3d& ecef) 100 | { 101 | return x_e2n.transformp(ecef); 102 | } 103 | 104 | static void ecef2ned(const xform::Xformd x_e2n, const Eigen::Vector3d& ecef, Eigen::Vector3d& ned) 105 | { 106 | ned = x_e2n.transformp(ecef); 107 | } 108 | 109 | static void lla2ned(const Eigen::Vector3d& lla0, const Eigen::Vector3d& lla, Eigen::Vector3d& ned) 110 | { 111 | xform::Xformd x_e2n; 112 | x_e2n.q() = q_e2n(lla0); 113 | x_e2n.t() = lla2ecef(lla0); 114 | ecef2ned(x_e2n, lla2ecef(lla), ned); 115 | } 116 | 117 | static Eigen::Vector3d lla2ned(const Eigen::Vector3d& lla0, const Eigen::Vector3d& lla) 118 | { 119 | Eigen::Vector3d ned; 120 | lla2ned(lla0, lla, ned); 121 | return ned; 122 | } 123 | 124 | static void ned2lla(const Eigen::Vector3d& lla0, const Eigen::Vector3d& ned, Eigen::Vector3d&lla) 125 | { 126 | xform::Xformd x_e2n; 127 | x_e2n.q() = q_e2n(lla0); 128 | x_e2n.t() = lla2ecef(lla0); 129 | ecef2lla(ned2ecef(x_e2n, ned), lla); 130 | } 131 | 132 | static Eigen::Vector3d ned2lla(const Eigen::Vector3d& lla0, const Eigen::Vector3d& ned) 133 | { 134 | Eigen::Vector3d lla; 135 | ned2lla(lla0, ned, lla); 136 | return lla; 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /roscopter_utils/include/roscopter_utils/input_parser.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | 7 | namespace roscopter 8 | { 9 | class InputParser{ 10 | public: 11 | InputParser (int &argc, char **argv) 12 | { 13 | for (int i=1; i < argc; ++i) 14 | this->tokens.push_back(std::string(argv[i])); 15 | } 16 | 17 | template 18 | bool getCmdOption(const std::string &option, T& ret) const 19 | { 20 | std::stringstream ss; 21 | auto itr = std::find(this->tokens.begin(), this->tokens.end(), option); 22 | if (itr != this->tokens.end() && ++itr != this->tokens.end()) 23 | { 24 | ss << *itr; 25 | ss >> ret; 26 | return true; 27 | } 28 | return false; 29 | } 30 | 31 | bool cmdOptionExists(const std::string &option) const 32 | { 33 | return std::find(this->tokens.begin(), this->tokens.end(), option) 34 | != this->tokens.end(); 35 | } 36 | 37 | private: 38 | std::vector tokens; 39 | }; 40 | } 41 | -------------------------------------------------------------------------------- /roscopter_utils/include/roscopter_utils/logger.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #define LOGGER_BUFFER_SIZE 1024*1024 13 | 14 | namespace roscopter 15 | { 16 | 17 | class Logger 18 | { 19 | public: 20 | Logger() {} 21 | 22 | Logger(const std::string filename) 23 | { 24 | open(filename); 25 | } 26 | 27 | void open(const std::string& filename) 28 | { 29 | file_.open(filename); 30 | } 31 | 32 | ~Logger() 33 | { 34 | file_.close(); 35 | } 36 | template 37 | void log(T... data) 38 | { 39 | int dummy[sizeof...(data)] = { (file_.write((char*)&data, sizeof(T)), 1)... }; 40 | } 41 | 42 | template 43 | void logVectors(T... data) 44 | { 45 | int dummy[sizeof...(data)] = { (file_.write((char*)data.data(), sizeof(typename T::Scalar)*data.rows()*data.cols()), 1)... }; 46 | } 47 | 48 | std::ofstream file_; 49 | }; 50 | 51 | } 52 | 53 | 54 | -------------------------------------------------------------------------------- /roscopter_utils/include/roscopter_utils/progress_bar.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | class ProgressBar 7 | { 8 | public: 9 | ProgressBar(){} 10 | ProgressBar(int total, int barwidth) : 11 | initialized_(false), 12 | barwidth_(barwidth), 13 | total_(total) 14 | {} 15 | 16 | ~ProgressBar() 17 | { 18 | // std::cout << std::endl; 19 | } 20 | 21 | void init(int total, int barwidth) 22 | { 23 | initialized_ = false; 24 | barwidth_ = barwidth; 25 | total_ = total; 26 | last_completed_ = 0; 27 | } 28 | 29 | void set_theme_line() { bars_ = {"─", "─", "─", "╾", "╾", "╾", "╾", "━", "═"}; } 30 | void set_theme_circle() { bars_ = {" ", "◓", "◑", "◒", "◐", "◓", "◑", "◒", "#"}; } 31 | void set_theme_braille() { bars_ = {" ", "⡀", "⡄", "⡆", "⡇", "⡏", "⡟", "⡿", "⣿" }; } 32 | void set_theme_braille_spin() { bars_ = {" ", "⠙", "⠹", "⠸", "⠼", "⠴", "⠦", "⠇", "⠿" }; } 33 | 34 | void print(int completed, double t=NAN) 35 | { 36 | if (!initialized_) 37 | { 38 | last_print_time_ = std::chrono::system_clock::now(); 39 | start_time_ = std::chrono::system_clock::now(); 40 | initialized_ = true; 41 | t_start_ = t; 42 | elapsed_last_ = 0.0; 43 | t_last_ = t; 44 | rt_rate_ = 1.0; 45 | rt_alpha_ = 0.9; 46 | } 47 | std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); 48 | 49 | // limit printing to about 30 Hz 50 | if (std::chrono::duration_cast(now - last_print_time_).count() > 33 51 | || completed == total_) 52 | { 53 | double elapsed = std::chrono::duration_cast(now - start_time_).count() / 1000.0; 54 | last_print_time_ = now; 55 | std::cout << " \r ["; 56 | double pos = barwidth_ * (completed / (double)total_); 57 | for (int i = 0; i < barwidth_; ++i) 58 | if (i < floor(pos)) std::cout << *(bars_.end()-1); 59 | else if (i == floor(pos)) std::cout << bars_[round((pos - floor(pos)) * (bars_.size() -1))]; 60 | else std::cout << " "; 61 | std::cout << "] "; 62 | printf("%.0f%% ", (completed / (double)total_)*100.0); 63 | double it_s = completed / elapsed; 64 | std::string left_stamp = ms_to_stamp(((total_ - completed) / it_s)*1000); 65 | std::string elapsed_stamp = ms_to_stamp(elapsed * 1000.0); 66 | if (std::isfinite(t_start_) && std::isfinite(t)) 67 | { 68 | double rt_factor = (t - t_last_) / (elapsed - elapsed_last_); 69 | rt_rate_ = rt_alpha_ * rt_rate_ + (1.0 - rt_alpha_) * rt_factor; 70 | t_last_ = t; 71 | elapsed_last_ = elapsed; 72 | printf("[%s<%s, %.2fit/s, %.2fx] ", elapsed_stamp.c_str(), left_stamp.c_str(), it_s, rt_rate_); 73 | } 74 | else 75 | { 76 | printf("[%s<%s, %.2fit/s] ", elapsed_stamp.c_str(), left_stamp.c_str(), it_s); 77 | } 78 | std::cout.flush(); 79 | } 80 | last_completed_ = completed; 81 | t_end_ = t; 82 | } 83 | 84 | void finished() 85 | { 86 | print(total_, t_end_); 87 | } 88 | private: 89 | 90 | std::string ms_to_stamp(int ms) 91 | { 92 | if (ms <= 0.0) 93 | { 94 | return ""; 95 | } 96 | int millis = ms % 1000; 97 | int sec = ((ms - millis) % (60 * 1000)) / 1000; 98 | int min = ((ms - (millis + sec*1000)) % (60 * 60 * 1000)) / (60*1000); 99 | int hour = ((ms - (millis + (sec + min*60)*1000)) % (24 * 60 * 60 * 1000)) / (60*60*1000); 100 | int day = ((ms - (millis + (sec + (min + hour * 60) * 60) * 1000)) / (24 * 60 * 60 * 1000))/(24*60*60*1000); 101 | char buf[25]; 102 | int n; 103 | if (day > 0) 104 | n = sprintf(buf, "%d:%d:%02d:%02d:%03d", day, hour, min, sec, millis); 105 | else if (hour > 0) 106 | n = sprintf(buf, "%d:%02d:%02d:%03d", hour, min, sec, millis); 107 | else if (min > 0) 108 | n = sprintf(buf, "%d:%02d:%03d", min, sec, millis); 109 | else if (sec > 0) 110 | n = sprintf(buf, "%d:%03d", sec, millis); 111 | else 112 | n = sprintf(buf, "%d", millis); 113 | std::string out(buf); 114 | return out; 115 | } 116 | 117 | 118 | 119 | double rt_alpha_; 120 | double rt_rate_; 121 | double t_last_; 122 | double elapsed_last_; 123 | int barwidth_; 124 | int total_; 125 | bool initialized_; 126 | int last_completed_; 127 | double t_start_; 128 | double t_end_; 129 | std::vector bars_ = {" ", "▏", "▎", "▍", "▋", "▋", "▊", "▉", "▉", "█"}; 130 | 131 | std::chrono::system_clock::time_point start_time_; 132 | std::chrono::system_clock::time_point last_print_time_; 133 | }; 134 | -------------------------------------------------------------------------------- /roscopter_utils/include/roscopter_utils/yaml.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace roscopter 10 | { 11 | 12 | template 13 | bool get_yaml_node(const std::string key, const std::string filename, T& val, const std::string node_namespace = "", bool print_error = true) 14 | { 15 | // Try to load the YAML file 16 | YAML::Node node; 17 | try 18 | { 19 | node = YAML::LoadFile(filename); 20 | if (node_namespace != ""){ 21 | node = node[node_namespace]; 22 | } 23 | } 24 | catch (...) 25 | { 26 | std::cout << "Failed to Read yaml file " << filename << std::endl; 27 | } 28 | 29 | // Throw error if unable to load a parameter 30 | if (node[key]) 31 | { 32 | val = node[key].as(); 33 | return true; 34 | } 35 | else 36 | { 37 | if (print_error) 38 | { 39 | throw std::runtime_error("Unable to load " + key + " from " + filename); 40 | } 41 | return false; 42 | } 43 | } 44 | 45 | template 46 | bool get_yaml_eigen(const std::string key, const std::string filename, Eigen::MatrixBase& val,const std::string node_namespace = "", bool print_error=true) 47 | { 48 | // Try to load the YAML file 49 | YAML::Node node; 50 | try 51 | { 52 | node = YAML::LoadFile(filename); 53 | if (node_namespace != ""){ 54 | node = node[node_namespace]; 55 | } 56 | } 57 | catch (...) 58 | { 59 | std::cout << "Failed to Read yaml file " << filename << std::endl; 60 | } 61 | 62 | std::vector vec; 63 | if (node[key]) 64 | { 65 | vec = node[key].as>(); 66 | if (vec.size() == (val.rows() * val.cols())) 67 | { 68 | int k = 0; 69 | for (int i = 0; i < val.rows(); i++) 70 | { 71 | for (int j = 0; j < val.cols(); j++) 72 | { 73 | val(i,j) = vec[k++]; 74 | } 75 | } 76 | return true; 77 | } 78 | else 79 | { 80 | throw std::runtime_error("Eigen Matrix Size does not match parameter size for " + key + " in " + filename + 81 | ". Requested " + std::to_string(Derived1::RowsAtCompileTime) + "x" + std::to_string(Derived1::ColsAtCompileTime) + 82 | ", Found " + std::to_string(vec.size())); 83 | return false; 84 | } 85 | } 86 | else if (print_error) 87 | { 88 | throw std::runtime_error("Unable to load " + key + " from " + filename); 89 | } 90 | return false; 91 | } 92 | 93 | template 94 | bool get_yaml_diag(const std::string key, const std::string filename, Eigen::MatrixBase& val,const std::string node_namespace = "", bool print_error=true) 95 | { 96 | Eigen::Matrix diag; 97 | if (get_yaml_eigen(key, filename, diag, node_namespace, print_error)) 98 | { 99 | val = diag.asDiagonal(); 100 | return true; 101 | } 102 | return false; 103 | } 104 | 105 | template 106 | bool get_yaml_priority(const std::string key, const std::string file1, const std::string file2, T& val , const std::string node_namespace = "") 107 | { 108 | if (get_yaml_node(key, file1, val,node_namespace, false)) 109 | { 110 | return true; 111 | } 112 | else 113 | { 114 | return get_yaml_node(key, file2, val,node_namespace, true); 115 | } 116 | } 117 | 118 | template 119 | bool get_yaml_priority_eigen(const std::string key, const std::string file1, const std::string file2, Eigen::MatrixBase& val , const std::string node_namespace = "") 120 | { 121 | if (get_yaml_eigen(key, file1, val,node_namespace, false)) 122 | { 123 | return true; 124 | } 125 | else 126 | { 127 | return get_yaml_eigen(key, file2, val,node_namespace, true); 128 | } 129 | } 130 | } 131 | -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/.gitignore: -------------------------------------------------------------------------------- 1 | build/* 2 | *.user 3 | -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/.travis.yml: -------------------------------------------------------------------------------- 1 | dist: "bionic" 2 | language: cpp 3 | 4 | addons: 5 | apt: 6 | packages: 7 | - build-essential 8 | - cmake 9 | - libgtest-dev 10 | - libeigen3-dev 11 | update: true 12 | 13 | before_install: 14 | - sudo apt update 15 | - sudo apt install -y libgtest-dev libeigen3-dev cmake build-essential libopencv-dev 16 | 17 | install: 18 | - cd /usr/src/gtest 19 | - sudo cmake CMakeLists.txt 20 | - sudo make 21 | - sudo cp *.a /usr/lib 22 | - cd "${TRAVIS_BUILD_DIR}" 23 | - gcc --version 24 | 25 | script: 26 | - mkdir build 27 | - cd build 28 | - cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTS=ON 29 | - make 30 | - ./geometry_test 31 | 32 | -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 3.7) 2 | project (geometry) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 5 | find_package(Eigen3 REQUIRED) 6 | 7 | add_library(geometry INTERFACE) 8 | target_include_directories(geometry INTERFACE 9 | $ 10 | $ 11 | ${EIGEN3_INCLUDE_DIRS}) 12 | 13 | option(BUILD_TESTS "Build Tests" OFF) 14 | if (BUILD_TESTS) 15 | find_package(GTest REQUIRED) 16 | find_package(OpenCV REQUIRED) 17 | add_executable(geometry_test src/test.cpp) 18 | target_include_directories(geometry_test PUBLIC include ${GTEST_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 19 | target_link_libraries(geometry_test ${GTEST_LIBRARIES} gtest gtest_main pthread ${OpenCV_LIBRARIES}) 20 | endif() 21 | 22 | 23 | set(LIB_DEST lib/${PROJECT_NAME}) 24 | set(INCLUDE_DEST include) 25 | 26 | install(TARGETS ${PROJECT_NAME} 27 | EXPORT ${PROJECT_NAME}-targets 28 | ARCHIVE DESTINATION ${LIB_DEST} 29 | LIBRARY DESTINATION ${LIB_DEST} 30 | ) 31 | install(DIRECTORY include/${PROJECT_NAME} 32 | DESTINATION ${INCLUDE_DEST} 33 | FILES_MATCHING PATTERN "*.h" 34 | ) 35 | 36 | # install CMake package configuration 37 | install(EXPORT ${PROJECT_NAME}-targets DESTINATION ${LIB_DEST}) 38 | install(FILES ${PROJECT_NAME}-config.cmake DESTINATION ${LIB_DEST}) 39 | -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.com/superjax/geometry.svg?branch=master)](https://travis-ci.com/superjax/geometry) 2 | -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/cmake/geometryConfig.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | 3 | include("${CMAKE_CURRENT_LIST_DIR}/geometryTargets.cmake") 4 | check_required_components("@PROJECT_NAME@") -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/geometry-config.cmake: -------------------------------------------------------------------------------- 1 | get_filename_component(SELF_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 2 | include(${SELF_DIR}/geometry-targets.cmake) 3 | get_filename_component(geometry_INCLUDE_DIRS "${SELF_DIR}/../../include/geometry" ABSOLUTE) 4 | -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/include/geometry/support.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | typedef Eigen::Matrix Vector1d; 8 | typedef Eigen::Matrix Vector5d; 9 | typedef Eigen::Matrix Vector6d; 10 | typedef Eigen::Matrix Vector7d; 11 | typedef Eigen::Matrix Vector8d; 12 | typedef Eigen::Matrix Vector9d; 13 | typedef Eigen::Matrix Vector10d; 14 | 15 | typedef Eigen::Matrix Matrix1d; 16 | typedef Eigen::Matrix Matrix5d; 17 | typedef Eigen::Matrix Matrix6d; 18 | typedef Eigen::Matrix Matrix7d; 19 | typedef Eigen::Matrix Matrix8d; 20 | typedef Eigen::Matrix Matrix9d; 21 | 22 | 23 | static const Eigen::Matrix I_2x3 = [] { 24 | Eigen::Matrix tmp; 25 | tmp << 1.0, 0, 0, 26 | 0, 1.0, 0; 27 | return tmp; 28 | }(); 29 | 30 | static const Eigen::Matrix3d I_3x3 = [] { 31 | Eigen::Matrix3d tmp = Eigen::Matrix3d::Identity(); 32 | return tmp; 33 | }(); 34 | 35 | static const Eigen::Matrix2d I_2x2 = [] { 36 | Eigen::Matrix2d tmp = Eigen::Matrix2d::Identity(); 37 | return tmp; 38 | }(); 39 | 40 | 41 | static const Eigen::Vector3d e_x = [] { 42 | Eigen::Vector3d tmp; 43 | tmp << 1.0, 0, 0; 44 | return tmp; 45 | }(); 46 | 47 | static const Eigen::Vector3d e_y = [] { 48 | Eigen::Vector3d tmp; 49 | tmp << 0, 1.0, 0; 50 | return tmp; 51 | }(); 52 | 53 | static const Eigen::Vector3d e_z = [] { 54 | Eigen::Vector3d tmp; 55 | tmp << 0, 0, 1.0; 56 | return tmp; 57 | }(); 58 | 59 | template 60 | inline Eigen::Matrix skew(const Eigen::MatrixBase& v) 61 | { 62 | Eigen::Matrix mat; 63 | typename Derived::Scalar zr(0.0); 64 | mat << zr, -v(2), v(1), 65 | v(2), zr, -v(0), 66 | -v(1), v(0), zr; 67 | return mat; 68 | } 69 | 70 | template 71 | void setNormalRandom(Eigen::MatrixBase& M, std::normal_distribution& N, std::default_random_engine& g) 72 | { 73 | for (int i = 0; i < M.rows(); i++) 74 | { 75 | for (int j = 0; j < M.cols(); j++) 76 | { 77 | M(i,j) = N(g); 78 | } 79 | } 80 | } 81 | 82 | template 83 | Derived randomNormal(std::normal_distribution& N, std::default_random_engine& g) 84 | { 85 | Derived out; 86 | for (int i = 0; i < Derived::RowsAtCompileTime; i++) 87 | { 88 | for (int j = 0; j < Derived::ColsAtCompileTime; j++) 89 | { 90 | out(i,j) = N(g); 91 | } 92 | } 93 | return out; 94 | } 95 | 96 | template 97 | Eigen::Matrix randomUniform(std::uniform_real_distribution& N, std::default_random_engine& g) 98 | { 99 | Eigen::Matrix out; 100 | for (int i = 0; i < R; i++) 101 | { 102 | for (int j = 0; j < C; j++) 103 | { 104 | out(i,j) = N(g); 105 | } 106 | } 107 | return out; 108 | } 109 | 110 | template 111 | int sign(T in) 112 | { 113 | return (in >= 0) - (in < 0); 114 | } 115 | 116 | template 117 | inline T random(T max, T min) 118 | { 119 | T f = (T)rand() / RAND_MAX; 120 | return min + f * (max - min); 121 | } 122 | -------------------------------------------------------------------------------- /roscopter_utils/lib/geometry/include/geometry/xform.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "geometry/support.h" 9 | #include "geometry/quat.h" 10 | 11 | namespace xform 12 | { 13 | 14 | template 15 | class Xform 16 | { 17 | private: 18 | 19 | typedef Eigen::Matrix Vec2; 20 | typedef Eigen::Matrix Vec3; 21 | typedef Eigen::Matrix Vec4; 22 | typedef Eigen::Matrix Vec5; 23 | typedef Eigen::Matrix Vec6; 24 | typedef Eigen::Matrix Vec7; 25 | 26 | typedef Eigen::Matrix Mat3; 27 | typedef Eigen::Matrix Mat4; 28 | typedef Eigen::Matrix Mat6; 29 | T buf_[7]; 30 | 31 | public: 32 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 33 | Eigen::Map arr_; 34 | Eigen::Map t_; 35 | quat::Quat q_; 36 | 37 | Xform() : 38 | arr_(buf_), 39 | t_(arr_.data()), 40 | q_(arr_.data() + 3) 41 | { 42 | arr_.setZero(); 43 | arr_(3) = (T)1.0; 44 | } 45 | 46 | Xform(const Eigen::Ref arr) : 47 | arr_(const_cast(arr.data())), 48 | t_(arr_.data()), 49 | q_(arr_.data() + 3) 50 | {} 51 | 52 | Xform(const Xform& X) : 53 | arr_(buf_), 54 | t_(arr_.data()), 55 | q_(arr_.data() + 3) 56 | { 57 | arr_ = X.arr_; 58 | } 59 | 60 | Xform(const T* data) : 61 | arr_(const_cast(data)), 62 | t_(arr_.data()), 63 | q_(arr_.data() + 3) 64 | {} 65 | 66 | Xform(const Vec3& t, const quat::Quat& q) : 67 | arr_(buf_), 68 | t_(arr_.data()), 69 | q_(arr_.data() + 3) 70 | { 71 | // copies arguments into contiguous (owned) memory 72 | t_ = t; 73 | q_ = q; 74 | } 75 | 76 | Xform(const Vec3& t, const Mat3& R) : 77 | arr_(buf_), 78 | t_(arr_.data()), 79 | q_(arr_.data() + 3) 80 | { 81 | q_ = quat::Quat::from_R(R); 82 | t_ = t; 83 | } 84 | 85 | T* data() 86 | { 87 | return arr_.data(); 88 | } 89 | 90 | const T* data() const 91 | { 92 | return arr_.data(); 93 | } 94 | 95 | // Xform(const Mat4& X) : 96 | // arr_(buf_), 97 | // t_(arr_.data()), 98 | // q_(arr_.data() + 3) 99 | // { 100 | // q_ = Quat::from_R(X.block<3,3>(0,0)); 101 | // t_ = X.block<3,1>(0, 3); 102 | // } 103 | 104 | inline Eigen::Map& t() { return t_;} 105 | inline const Eigen::Map& t() const { return t_;} 106 | inline quat::Quat& q() { return q_;} 107 | inline const quat::Quat& q() const { return q_;} 108 | inline Eigen::Map& arr() { return arr_; } 109 | inline const Eigen::Map& arr() const { return arr_; } 110 | inline void setq(const quat::Quat& q) {q_ = q;} 111 | inline void sett(const Vec3&t) {t_ = t;} 112 | 113 | Xform operator* (const Xform& X) const 114 | { 115 | return otimes(X); 116 | } 117 | 118 | Xform& operator*= (const Xform& X) 119 | { 120 | t_ = t_ + q_.rotp(X.t_); 121 | q_ = q_ * X.q_; 122 | return *this; 123 | } 124 | 125 | Xform& operator=(const Xform& X) 126 | { 127 | t_ = X.t_; 128 | q_ = X.q_; 129 | return *this; 130 | } 131 | 132 | Xform& operator=(const Vec7& v) 133 | { 134 | t_ = v.template segment<3>(0); 135 | q_ = quat::Quat(v.template segment<4>(3)); 136 | return *this; 137 | } 138 | 139 | Xform operator+ (const Vec6& v) const 140 | { 141 | return boxplus(v); 142 | } 143 | 144 | template 145 | Eigen::Matrix operator- (const Xform& X) const 146 | { 147 | return boxminus(X); 148 | } 149 | 150 | Xform& operator+=(const Vec6& v) 151 | { 152 | arr_ = boxplus(v).elements(); 153 | return *this; 154 | } 155 | 156 | template 157 | Xform cast() const 158 | { 159 | Xform x; 160 | x.arr_ = arr_.template cast(); 161 | return x; 162 | } 163 | 164 | Vec7 elements() const 165 | { 166 | Vec7 out; 167 | out.template block<3,1>(0,0) = t_; 168 | out.template block<4,1>(3,0) = q_.arr_; 169 | return out; 170 | } 171 | 172 | Mat4 Mat() const 173 | { 174 | Mat4 out; 175 | out.block<3,3>(0,0) = q_.R(); 176 | out.block<3,1>(0,3) = t_; 177 | out.block<1,3>(3,0) = Eigen::Matrix::Zero(); 178 | out(3,3) = 1.0; 179 | } 180 | 181 | static Xform Identity() 182 | { 183 | Xform out; 184 | out.t_.setZero(); 185 | out.q_ = quat::Quat::Identity(); 186 | return out; 187 | } 188 | 189 | static Xform Random() 190 | { 191 | Xform out; 192 | out.t_.setRandom(); 193 | out.q_ = quat::Quat::Random(); 194 | return out; 195 | } 196 | 197 | static Xform exp(const Eigen::Ref& v) 198 | { 199 | Vec3 u = v.template block<3,1>(0,0); 200 | Vec3 omega = v.template block<3,1>(3,0); 201 | T th = omega.norm(); 202 | quat::Quat q_exp = quat::Quat::exp(omega); 203 | if (th > 1e-4) 204 | { 205 | Mat3 wx = quat::Quat::skew(omega); 206 | T B = ((T)1. - cos(th)) / (th * th); 207 | T C = (th - sin(th)) / (th * th * th); 208 | return Xform((I_3x3 + B*wx + C*wx*wx).transpose() * u, q_exp); 209 | } 210 | else 211 | { 212 | return Xform(u, q_exp); 213 | } 214 | } 215 | 216 | static Vec6 log(const Xform& X) 217 | { 218 | Vec6 u; 219 | Vec3 omega = quat::Quat::log(X.q_); 220 | u.template block<3,1>(3,0) = omega; 221 | T th = omega.norm(); 222 | if (th > 1e-8) 223 | { 224 | Mat3 wx = quat::Quat::skew(omega); 225 | T A = sin(th)/th; 226 | T B = ((T)1. - cos(th)) / (th * th); 227 | Mat3 V = I_3x3 - (1./2.)*wx + (1./(th*th)) * (1.-(A/(2.*B)))*(wx* wx); 228 | u.template block<3,1>(0,0) = V.transpose() * X.t_; 229 | } 230 | else 231 | { 232 | u.template block<3,1>(0,0) = X.t_; 233 | } 234 | return u; 235 | } 236 | 237 | Mat6 Adj() const 238 | { 239 | Mat6 out; 240 | Mat3 R = q_.R(); 241 | out.template block<3,3>(0,0) = R; 242 | out.template block<3,3>(0,3) = quat::Quat::skew(t_)*R; 243 | out.template block<3,3>(3,3) = R; 244 | out.template block<3,3>(3,0) = Mat3::Zero(); 245 | return out; 246 | } 247 | 248 | Xform inverse() const{ 249 | Xform out(-q_.rotp(t_), q_.inverse()); 250 | return out; 251 | } 252 | 253 | template 254 | Xform otimes(const Xform& X2) const 255 | { 256 | Xform X; 257 | Eigen::Matrix t = (Tout)2.0*X2.t_.cross(q_.bar()); 258 | X.t_ = t_+ X2.t_ - q_.w()* t + t.cross(q_.bar()); 259 | X.q_ = q_.template otimes(X2.q_); 260 | return X; 261 | } 262 | 263 | template 264 | Eigen::Matrix transforma(const Derived& v) const 265 | { 266 | static_assert(Derived::RowsAtCompileTime == Eigen::Dynamic 267 | || Derived::RowsAtCompileTime == 3, 268 | "Can only transform 3x1 vectors"); 269 | static_assert(Derived::ColsAtCompileTime == Eigen::Dynamic 270 | || Derived::ColsAtCompileTime == 1, 271 | "Can only transform 3x1 vectors"); 272 | 273 | return q_.template rota(v) + t_; 274 | } 275 | 276 | template 277 | Eigen::Matrix transformp(const Derived& v) const 278 | { 279 | static_assert(Derived::RowsAtCompileTime == 3, 280 | "Can only transform 3x1 vectors"); 281 | static_assert(Derived::ColsAtCompileTime == 1, 282 | "Can only transform 3x1 vectors"); 283 | 284 | return q_.template rotp(v - t_); 285 | } 286 | 287 | template 288 | Eigen::Matrix rota(const Derived& v) const 289 | { 290 | return q_.template rota(v); 291 | } 292 | 293 | template 294 | Eigen::Matrix rotp(const Derived& v) const 295 | { 296 | return q_.template rotp(v); 297 | } 298 | 299 | Xform& invert() 300 | { 301 | t_ = -q_.rotp(t_); 302 | q_.invert(); 303 | return *this; 304 | } 305 | 306 | template 307 | Xform boxplus(const Eigen::Matrix& delta) const 308 | { 309 | return otimes(Xform::exp(delta)); 310 | } 311 | 312 | template 313 | Eigen::Matrix boxminus(const Xform& X) const 314 | { 315 | return Xform::log(X.inverse().otimes(*this)); 316 | } 317 | 318 | }; 319 | 320 | template 321 | inline std::ostream& operator<< (std::ostream& os, const Xform& X) 322 | { 323 | os << "t: [ " << X.t_(0,0) << ", " << X.t_(1,0) << ", " << X.t_(2,0) << 324 | "] q: [ " << X.q_.w() << ", " << X.q_.x() << "i, " << X.q_.y() << "j, " << X.q_.z() << "k]"; 325 | return os; 326 | } 327 | 328 | typedef Xform Xformd; 329 | 330 | } 331 | -------------------------------------------------------------------------------- /roscopter_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roscopter_utils 4 | 0.0.0 5 | Utilities to expand core functionality of ROScopter 6 | 7 | Parker Lusk 8 | 9 | TODO 10 | 11 | https://github.com/byu-magicc/roscopter 12 | https://github.com/byu-magicc/roscopter/issues 13 | 14 | catkin 15 | ament_cmake 16 | 17 | 18 | 19 | catkin 20 | ament_cmake 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /roscopter_utils/src/vimfly: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | vimfly - vim keybindings for your multirotor! 4 | 5 | Teleoperated flying from your keyboard. Command u, v, psidot, and altitude. 6 | 7 | The following keybindings are used: 8 | - a: lower altitude 9 | - s: higher altitude 10 | - d: CCW (-psidot) 11 | - f: CW (+psidot) 12 | - h: Left (-v) 13 | - j: Backward (-u) 14 | - k: Forward (+u) 15 | - l: Right (+v) 16 | 17 | Connect to rosflight_io command topic (SIL/HW) or roscopter_sim Gazebo command 18 | topic. 19 | 20 | 21 | https://xkcd.com/1823/ 22 | """ 23 | import pygame 24 | import rospy 25 | from rosflight_msgs.msg import Command 26 | 27 | 28 | class VimFly: 29 | def __init__(self): 30 | 31 | # initialize pygame display 32 | pygame.init() 33 | pygame.display.set_caption('vimfly') 34 | self.screen = pygame.display.set_mode((550, 200)) 35 | self.font = pygame.font.SysFont("monospace", 18) 36 | 37 | # Autopilot commands for HW, SIL, or roscopter_sim 38 | self.autopilot_cmd_pub = rospy.Publisher('autopilot_command', Command, queue_size=10) 39 | 40 | # retrieve vimfly parameters from the rosparam server 41 | self.params = { 42 | 'u_cmd': rospy.get_param('~u_cmd', 2.0), 43 | 'v_cmd': rospy.get_param('~v_cmd', 2.0), 44 | 'psidot_cmd': rospy.get_param('~psidot_cmd', 3.14159), 45 | 'altitude_cmd': rospy.get_param('~alt_start', -1.0), # start at this altitude 46 | 'altitude_step': rospy.get_param('~alt_step', 0.1), # each step inc/dec by this amount 47 | } 48 | 49 | # Allow continuous holding of altitude keys without growing too big 50 | self.alt_debouncing = False 51 | self.ALT_DEBOUNCE_THRESHOLD = 0.100 52 | 53 | # Send commands 'til I die 54 | self.run() 55 | 56 | 57 | def run(self): 58 | rate = rospy.Rate(30) 59 | while not rospy.is_shutdown(): 60 | 61 | # initialize command message 62 | msg = Command() 63 | msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE 64 | 65 | keys = pygame.key.get_pressed() 66 | 67 | # LEFT -- h 68 | if keys[pygame.K_h]: 69 | msg.y = -self.params['v_cmd'] 70 | 71 | # RIGHT -- l 72 | elif keys[pygame.K_l]: 73 | msg.y = self.params['v_cmd'] 74 | 75 | else: 76 | msg.y = 0 77 | 78 | 79 | # FORWARD -- k 80 | if keys[pygame.K_k]: 81 | msg.x = self.params['u_cmd'] 82 | 83 | # BACKWARD -- j 84 | elif keys[pygame.K_j]: 85 | msg.x = -self.params['u_cmd'] 86 | 87 | else: 88 | msg.x = 0 89 | 90 | 91 | # CCW -- d 92 | if keys[pygame.K_d]: 93 | msg.z = -self.params['psidot_cmd'] 94 | 95 | # CW -- f 96 | elif keys[pygame.K_f]: 97 | msg.z = self.params['psidot_cmd'] 98 | 99 | else: 100 | msg.z = 0 101 | 102 | 103 | # LOWER -- a // HIGHER -- s 104 | if keys[pygame.K_a] or keys[pygame.K_s]: 105 | if not self.alt_debouncing: 106 | self.alt_debouncing = True 107 | self.alt_start_time = rospy.get_time() 108 | 109 | if (rospy.get_time() - self.alt_start_time) > self.ALT_DEBOUNCE_THRESHOLD: 110 | 111 | # The key has been debounced once, start the process over! 112 | self.alt_debouncing = False 113 | 114 | # Increment the commanded altitude 115 | if keys[pygame.K_a]: 116 | self.params['altitude_cmd'] += self.params['altitude_step'] 117 | 118 | elif keys[pygame.K_s]: 119 | self.params['altitude_cmd'] -= self.params['altitude_step'] 120 | 121 | else: 122 | self.alt_debouncing = False 123 | 124 | 125 | # Always send an altitude command -- we don't want to drop like a brick! 126 | msg.F = self.params['altitude_cmd'] 127 | self.autopilot_cmd_pub.publish(msg) 128 | 129 | # Update the display with the current commands 130 | self.update_display(msg) 131 | 132 | # process event queue and throttle the while loop 133 | pygame.event.pump() 134 | rate.sleep() 135 | 136 | 137 | def update_display(self, msg): 138 | self.display_help() 139 | 140 | msgText = "u: {}, v: {}, psidot: {}, alt: {}".format(msg.x, msg.y, msg.z, msg.F) 141 | self.render(msgText, (0,140)) 142 | 143 | pygame.display.flip() 144 | 145 | 146 | def display_help(self): 147 | self.screen.fill((0,0,0)) 148 | 149 | LINE=20 150 | 151 | self.render("vimfly keybindings:", (0,0)) 152 | self.render("- a: lower altitude", (0,1*LINE)); self.render("- h: Left (-v)", (250,1*LINE)) 153 | self.render("- s: higher altitude", (0,2*LINE)); self.render("- j: Backward (-u)", (250,2*LINE)) 154 | self.render("- d: CCW (-psidot)", (0,3*LINE)); self.render("- k: Forward (+u)", (250,3*LINE)) 155 | self.render("- f: CW (+psidot)", (0,4*LINE)); self.render("- l: Right (+v)", (250,4*LINE)) 156 | 157 | 158 | def render(self, text, loc): 159 | txt = self.font.render(text, 1, (255,255,255)) 160 | self.screen.blit(txt, loc) 161 | 162 | 163 | 164 | if __name__ == '__main__': 165 | rospy.init_node('vimfly', anonymous=False) 166 | try: 167 | teleop = VimFly() 168 | except rospy.ROSInterruptException: 169 | pass -------------------------------------------------------------------------------- /wiki_resources/ROScopterArchitecure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/wiki_resources/ROScopterArchitecure.png -------------------------------------------------------------------------------- /wiki_resources/ROScopterLogo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/wiki_resources/ROScopterLogo.png -------------------------------------------------------------------------------- /wiki_resources/ROScopter_control_modes.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/wiki_resources/ROScopter_control_modes.pdf -------------------------------------------------------------------------------- /wiki_resources/ROScopter_control_modes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/wiki_resources/ROScopter_control_modes.png -------------------------------------------------------------------------------- /wiki_resources/ROSflightLogo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/wiki_resources/ROSflightLogo.png -------------------------------------------------------------------------------- /wiki_resources/ROSplaneLogo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/wiki_resources/ROSplaneLogo.png -------------------------------------------------------------------------------- /wiki_resources/rosflight_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/byu-magicc/roscopter/da60a78cfc52393b53148a102f375e3e2dc93627/wiki_resources/rosflight_logo.png --------------------------------------------------------------------------------