├── .ci ├── build.sh └── install.sh ├── .fig └── thumbnail.jpg ├── .github └── workflows │ └── noetic.yml ├── .gitignore ├── .gitmodules ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg └── odometry_dynparam.cfg ├── config ├── aloamgarm.yaml ├── altitude_estimator.yaml ├── debug_verbosity.yaml ├── default_config.yaml ├── heading_estimator.yaml ├── init_pose │ └── init_pose.csv ├── rtk_republisher.yaml ├── simulation │ ├── aloam.yaml │ ├── brick.yaml │ ├── brickflow.yaml │ ├── darpa.yaml │ ├── gps.yaml │ ├── hector.yaml │ ├── icp.yaml │ ├── liosam.yaml │ ├── optflow.yaml │ ├── rtk.yaml │ ├── simulation_default.yaml │ ├── vio.yaml │ └── vslam.yaml ├── state_estimators.yaml └── uav │ ├── aloam.yaml │ ├── darpa.yaml │ ├── gps.yaml │ ├── hector.yaml │ ├── liosam.yaml │ ├── optflow.yaml │ ├── rtk.yaml │ ├── uav_default.yaml │ ├── uwb.yaml │ ├── vio.yaml │ ├── vio_testing.yaml │ └── vslam.yaml ├── include ├── AltitudeEstimator.h ├── AltitudeEstimatorAloamGarm.h ├── HeadingEstimator.h ├── StateEstimator.h ├── support.h └── types.h ├── launch ├── gazebo │ ├── gazebo_altitude.launch │ └── gazebo_grass_plane.launch ├── odometry.launch ├── rtk_republisher.launch └── uwb_tf.launch ├── package.xml ├── plot_juggler └── odometry.xml ├── plugins.xml ├── rviz └── odometry.rviz ├── scripts ├── change_uav.sh └── simulation.sh └── src ├── AltitudeEstimator.cpp ├── AltitudeEstimatorAloamGarm.cpp ├── HeadingEstimator.cpp ├── StateEstimator.cpp ├── odometry.cpp └── rtk_republisher.cpp /.ci/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | distro=`lsb_release -r | awk '{ print $2 }'` 5 | [ "$distro" = "18.04" ] && ROS_DISTRO="melodic" 6 | [ "$distro" = "20.04" ] && ROS_DISTRO="noetic" 7 | 8 | echo "Starting build" 9 | cd ~/mrs_workspace 10 | source /opt/ros/$ROS_DISTRO/setup.bash 11 | catkin build --limit-status-rate 0.2 --summarize 12 | echo "Ended build" 13 | -------------------------------------------------------------------------------- /.ci/install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | 4 | distro=`lsb_release -r | awk '{ print $2 }'` 5 | [ "$distro" = "18.04" ] && ROS_DISTRO="melodic" 6 | [ "$distro" = "20.04" ] && ROS_DISTRO="noetic" 7 | 8 | # get the path to this script 9 | MY_PATH=`pwd` 10 | 11 | echo "Starting install" 12 | 13 | # get the current package name 14 | PACKAGE_NAME=${PWD##*/} 15 | 16 | sudo apt-get -y install git 17 | 18 | echo "clone uav_core" 19 | cd 20 | git clone https://github.com/ctu-mrs/uav_core.git 21 | cd uav_core 22 | 23 | echo "running the main install.sh" 24 | ./installation/install.sh 25 | 26 | gitman update 27 | 28 | # link the up-to-date version of this package 29 | rm -rf ~/uav_core/.gitman/$PACKAGE_NAME 30 | ln -s "$MY_PATH" ~/uav_core/.gitman/$PACKAGE_NAME 31 | 32 | mkdir -p ~/mrs_workspace/src 33 | cd ~/mrs_workspace/src 34 | ln -s ~/uav_core 35 | source /opt/ros/$ROS_DISTRO/setup.bash 36 | cd ~/mrs_workspace 37 | 38 | echo "install ended" 39 | -------------------------------------------------------------------------------- /.fig/thumbnail.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctu-mrs/mrs_uav_odometry/0c3eff9022b59fd355aee87a4d98b044ca39815f/.fig/thumbnail.jpg -------------------------------------------------------------------------------- /.github/workflows/noetic.yml: -------------------------------------------------------------------------------- 1 | name: Noetic 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | paths-ignore: 7 | - '**/README.md' 8 | - '**.yaml' 9 | - '**.yml' 10 | - '**.launch' 11 | pull_request: 12 | branches: [ master ] 13 | 14 | # Allows you to run this workflow manually from the Actions tab 15 | workflow_dispatch: 16 | 17 | jobs: 18 | 19 | cancel: 20 | 21 | name: Cancel Previous Runs 22 | runs-on: ubuntu-latest 23 | steps: 24 | - name: Cancel Previous Runs 25 | uses: styfle/cancel-workflow-action@0.11.0 26 | with: 27 | access_token: ${{ github.token }} 28 | 29 | build: 30 | runs-on: ubuntu-20.04 31 | 32 | steps: 33 | 34 | # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it 35 | - uses: actions/checkout@v3 36 | with: 37 | fetch-depth: 0 # fetch the whole history 38 | 39 | - name: Install 40 | run: ./.ci/install.sh 41 | 42 | - name: Build 43 | run: ./.ci/build.sh 44 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # ignore temp files 3 | *~ 4 | 5 | # ignore vim temp files 6 | *.swp 7 | *.swo 8 | *.swn 9 | 10 | # Runtime-python 11 | *.pyc 12 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ctu-mrs/mrs_uav_odometry/0c3eff9022b59fd355aee87a4d98b044ca39815f/.gitmodules -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mrs_uav_odometry 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.4 (2023-01-20) 6 | ------------------ 7 | * skipping non-height measurements before the first height measurement is received in repredictor altitude estimators (`#15 `_) 8 | Co-authored-by: Vaclav Pritzl 9 | * updated readme, updated ci 10 | * fixed possible takeoff with sonar and no garmin 11 | * fixed launch, env->optenv 12 | * fixed ambiguous use of attitude converter 13 | * not counting 0 to rtk avg position during initialization 14 | * fixed wrong safety area height 15 | * commented out unnecessary print 16 | * Revert "updated legacy heading calculation to use mrs_lib" This reverts commit 2a1f10076902fcd5dabea3446a7eb3b14cd91660. 17 | * updated legacy heading calculation to use mrs_lib 18 | * publishing global altitude in utm_origin tf 19 | * print warning when gps becomes unavailable before takeoff 20 | * inverted amsl transform 21 | * added amsl tfs 22 | * [tf connector]: forgot to remove some files 23 | * [tf connector]: moved to mrs_utils to not spam this repo with unrelated commits 24 | * [tf_connector]: it's now possible to specify offsets of the specific frames 25 | * Merge pull request `#14 `_ from ctu-mrs/aloamgarm 26 | * added new line to trigger github build 27 | * removed some commented out stuff from aloamgarm 28 | * using repredictor_aloamgarm for ALOAMGARM altitude estimator 29 | * [TFConnector]: fixes for various weird edge cases 30 | * [TFConnector]: rewrote it a bit to handle a mix of static and non-static TFs 31 | * [TFConnector]: minor fix with timestamps 32 | * [TfConnector]: Now also with static frames! Get your TF connector now and get one extra for free!! 33 | * updated changelog, updated version in configs 34 | * uwb estimator tested with rosbag 35 | * removed gps from aloam config 36 | * decreased the timeout of rtk averaging 37 | * wait longer until RTK altitude average converges during initialization 38 | * median filter fix 39 | * refactored reserved variable names 40 | * put back one probably redundant check 41 | * refactored to use the new median filter 42 | * takeoff should not be possible after loosing GPS 43 | * refactored against publisher handler 44 | * updated transformer interface 45 | * deleted old aloamgarm version, replaced with the new one 46 | * + install in cmakelists 47 | * add ScopeTimer to important methods 48 | * Removed unnecessary offsetting baro altitude during takeoff. Makes 49 | * fixed opflow + pixhawk estimator combination 50 | * aloamgarm - polishing code 51 | * fixed drifting baro estimator 52 | * checking list of active altitude estimators instead of all altitude estimators when changing altitude estimator 53 | * updated aloamgarm params 54 | * aloamgarm update - posibility to enter initial lkf conditions for debug 55 | * aloamgarm - filtering invalid measurements, setting larger R for measurements above 4 m 56 | * testing new version of aloamgarm 57 | * uwb estimator tested with rosbag 58 | * removed gps from aloam config 59 | * decreased the timeout of rtk averaging 60 | * wait longer until RTK altitude average converges during initialization 61 | * median filter fix 62 | * refactored reserved variable names 63 | Median refactor 64 | * put back one probably redundant check 65 | * refactored to use the new median filter 66 | * refactored against publisher handler 67 | * updated transformer interface 68 | * + install in cmakelists 69 | * add ScopeTimer to important methods 70 | * Removed unnecessary offsetting baro altitude during takeoff. Makes 71 | midair activation with baro estimator possible 72 | * fixed opflow + pixhawk estimator combination 73 | * fixed drifting baro estimator 74 | * checking list of active altitude estimators instead of all altitude estimators when changing altitude estimator 75 | * Contributors: Matej Petrlik, Matouš Vrba, Pavel Petracek, Tomas Baca, Vaclav Pritzl, matemat13 76 | 77 | 1.0.2 (2021-10-04) 78 | ------------------ 79 | * setting initial values for height estimator statecov 80 | * updated covariances for aloam (after Pilsen testing) 81 | * fixed a bug with altitude estimators still being initialized despite not 82 | being in active list 83 | * updated aloam covariances after 10 Hz aloam corrections 84 | * corrections for repredictor are fused only when new measurements are 85 | available 86 | * not publishing height when garmin not enabled 87 | * added fixed_map_origin frame 88 | * altitude estimators are now specified in the same way as lateral and 89 | heading 90 | * fixed wrong altitude of rtk_origin 91 | * aloamrep instead of aloam as default in simulation 92 | * fixed bug with invalid attitude with aloamrep in simulation 93 | * fallback to BARO from RTK 94 | * added id to max altitude status msg 95 | * fixed bug with overwritten yaml params 96 | * checking pixhwak quaternion for invalid values 97 | * updated height estimator to fuse measurements even when not suitable for 98 | altitude estimators 99 | * publishing max_altitude status msg 100 | * updated max allowed altitude to prevent flying higher with 101 | garmin-based estimator 102 | * fixed and tested fallback from aloam* estimators to gps, removed 103 | redundant code 104 | * changed median filter of aloam from 20s to 2s (responsible for a few 105 | manual takeovers in byci skala) 106 | * rtk altitude working on real hw 107 | * rtk to gps switching 108 | * added possibility to use full rtk estimator 109 | * updated mass loading for simulation 110 | * Added safety check to prevent use of multiple SLAM estimators when _use_general_slam_origin\_ is active 111 | * fixed ALOAMGARM (but ALOAMREP/ALOAMGARM still doesnt work when ALOAM state estimator is also active) 112 | * SLAMs: we now use default origin with name '/slam_origin' and input slam odometry is remappod to 'slam/odom' by default 113 | * aloam config now uses aloamrep internally 114 | * Contributors: Matej Petrlik, Pavel Petracek, Tomas Baca, Vaclav Pritzl 115 | 116 | 1.0.1 (2021-05-16) 117 | ------------------ 118 | * version -> 1.0.1 119 | * update liosam covariances after first flight testing 120 | * update liosam covariances 121 | * also allow to change liosam_origin to slam_origin 122 | * updated ros::shutdown 123 | * possibility to change aloam/liosam origin to slam_origin 124 | * fixed rtk altitude intialization (tested on one rosbag only) 125 | * fix in rtk saturate print 126 | * fixed double -> bool variables 127 | * updated initialization of gps_origin (update your world files!) 128 | * added heading estimator to diagnostics msg 129 | * increased repredictor buffer size, added AloamgarmDebug message publishing 130 | * added remaining median filter config params for ALOAMGARM 131 | * fixed Q matrix initilization in ALOAMGARM 132 | * removed unnecessary if 133 | * do not run repredictor without aloam data, fixes broken GPS flight 134 | * throttled "longer than 10 ms prediction" warning 135 | * aloamgarm update - separated aloam eigenvalue from odometry message, fixed dynamic reconfigure 136 | * polishing aloamgarm 137 | * removed commented out stuff from aloamgarm 138 | * removed some garbage related to ALOAMGARM 139 | * fixed aloam mapping tf 140 | * modified aloamgarm duration debug publishing 141 | * aloamgarm - added some debug publishers for processing duration and aloam_ok 142 | * aloamgarm - added state for barometer bias 143 | * increased toleration for aloamgarm median filter when close to the ground 144 | * aloamgarm update 145 | * aloamgarm - multiple values of difference for median filter 146 | * trying ALOAM + garmin fusion 147 | * passing zero input to estimators when attitude command is too old (to prevent repredictor warnings) 148 | * disabled initialization of local odometry offset to current UAV height 149 | * modified the rest of methods in HeadingEstimator to work with repredictor 150 | * passing current time to prediction when no controller is active to prevent repredictor warnings 151 | * added repredictor for StateEstimator and HeadingEstimator 152 | * publishing ALOAM delay on a topic 153 | * added repredictor reset 154 | * altitude estimation working with repredictor (ALOAMREP estimator) 155 | * repredictor for altitude estimation, it's broken 156 | * trying repredictor for altitude estimation 157 | * publishing debug topics, fixed switch to garmin 158 | * aloamgarm - fusing garmin differentially 159 | * trying aloam + garmin fusion for altitude estimation 160 | * Contributors: Matej Petrlik, Pavel Petracek, Tomas Baca, Vaclav Pritzl 161 | 162 | 1.0.0 (2021-03-18) 163 | ------------------ 164 | * Major release 165 | 166 | 0.0.6 (2021-03-16) 167 | ------------------ 168 | * Major refactoring and overhaul 169 | * c++ refactoring 170 | * the use of AttitudeConverter 171 | * heading-oriented estimation 172 | * new estimators 173 | * Noetic-compatible 174 | * Contributors: Jan Bednar, Matej Petrlik, Pavel Petracek, Robert Penicka, Tomas Baca, Vaclav Pritzl 175 | 176 | 0.0.5 (2020-02-26) 177 | ------------------ 178 | * updated declinging invalid garmin measurement 179 | * declining invladi garmin measurements 180 | * service call routines, waiting for map reset before estimator reset 181 | * checking if hector is running when switching to hector hdg 182 | * reset hector service returns false when hector not running 183 | * resetting acc state when reset hector 184 | * Contributors: Matej Petrlik 185 | 186 | 0.0.4 (2020-02-18) 187 | ------------------ 188 | * shorter median filter 189 | * plane cov 100->10 pls keep this value, higher breaks height estimator 190 | * hiccup and failed servoing printing 191 | * better mutexing of max_altitude, better diagnostic print 192 | * Plane will not be set to unreliable ever 193 | * not performing odometry switch if already active 194 | * height estimate published in fcu_untilted frame 195 | * fallback to optflow from gps 196 | * lowered covariance of icp 197 | * fixed taking off with plane estimator 198 | * fixed brick heading deadlock 199 | * updated dynamic reconfigure sequence 200 | * removed covariances overrding default ones 201 | * not fusing mavros velocity 202 | * add icp2d to allowed state estimators for aloam 203 | * gps, optflow, hector work reasonably well with se3 in simulation 204 | * three states 205 | * reset hector service 206 | * diagonal matrices 207 | * not switching estimators when already active 208 | * triggering control update during brick reset only when brick in feedback 209 | * simulation script switches worlds 210 | * switching to garmin when plane height above 4 m 211 | * add aloam_available to diagnostics msg 212 | * fixed awiting for tf in callbackGramin, rmeove timeouts 213 | * max heading brick jump 214 | * hiccup detected is now a warning 215 | * sanitized garmin callback 216 | * better mutexing in mainTimer 217 | * less uninitialized quaternion spam 218 | * brick max jump 219 | * checking nans and uninit quaternion in attitude cmd 220 | * publishing imu in fcu untilted 221 | * using attitude command from control manager instead of mavros target att 222 | * detecting max brick jump 223 | * saturating brick heading, slower fusion 224 | * odom stable using transformer 225 | * stable origin tf 226 | * moved odom_stable transform after tf is publshed 227 | * plane source for brick odometry 228 | * separated local_origin and stable_origin 229 | * brick timeout 0.5 s 230 | * rinfo uppercase lateral estimator 231 | * local_origin should work as intended wven with more active estimators 232 | * fixed deadlock when switching to non-active state estimator 233 | * correct baro offset when on the ground 234 | * height in odom_gps is from the current alt estimator 235 | * local_origin init 236 | * fixed garmin remap 237 | * added default value to GARMIN_FILTERED 238 | * local_origin should start at correct height 239 | * Change garmin input accoring to GARMIN_FILTERED 240 | * moved spam to ros_debug 241 | * local_origin is where the odometry node is started 242 | * initialization of rtk estimator 243 | * baro altitude estimator 244 | * brick_origin uses plane altitude estimator 245 | * service callback are now disabled by default 246 | * Add ALOAM as lateral, heading and height estimator 247 | * added service for toggling service callbacks 248 | * odom_aux now published in main_timer 249 | * cleaned up mainTimer 250 | * improved intialization of local_origin frame 251 | * Add altitude Q to config 252 | * removed altitude_world 253 | * added -Wall to check forgotten return types 254 | * brick timeout 255 | * max height for brick and plane height estimators 256 | * brick altitude switching 257 | * added brick_timeout to config 258 | * updated garmin median filter 259 | * saturating garmin corrections when toggled from off to on 260 | * tuning altitude estimation (not tested with real UAV) 261 | * tuned simulation covariances, publishing mavros odom 262 | * Add covariances of aloam to dynamic reconfigure 263 | * updated t650 mass in launch files 264 | * mrs_rviz_interface in simulation.sh 265 | * added world file resolving code to launch file 266 | * added WORLD_NAME parameter to launch file 267 | * Added missing dynamic reconfigure parameters 268 | * swapped order of checks with garmin 269 | * fixed untilted frame 270 | * preparing for optflow optimized for low altitude 271 | * Add tested version of AltitudeEstimator::ALOAM 272 | * fixed untilted frame 273 | * Add aloam_available to config 274 | * latlon definition of local origin 275 | * add aloam slam as new estimator 276 | * unified parameters common to uav and simulation into one config 277 | * fixed wrong brick height preventing postion fusion 278 | * [TFConnectorDummy]: trees should be connected through GPS origin and not local origin 279 | * [TFConnectorDummy]: added trycatch to lookuptransform to avoid crashes 280 | * brickflow altitude 281 | * added tf_connector_dummy for trivial connecting of TF trees between UAVs 282 | * brickflow implemented, needs tuning 283 | * prediction step triggered by main timer 284 | * added utm tf publisher 285 | * altitude estimator switch bugfix 286 | * height estimator when brick becomes unreliable 287 | * changed brick reliability check 288 | * fixing height when brick unreliable 289 | * fixed flying below 0 height with vio 290 | * fixed wrong frame of gps_local_odom 291 | * fix orientation in odom_stable 292 | * constant prediction rate 293 | * fixing BRICKFLOW estimator 294 | * increased aux publisher rate 295 | * no predictions when brick unreliable 296 | * no more nans in tfs 297 | * fixed for publishing local origin tf 298 | * fixed checking name of estimator 299 | * fcu tf moves again 300 | * fixed altitude in aux estimators 301 | * hopefully pass_rtk_as_odom works now 302 | * fixed tf when using ground truth 303 | * untilted is not unheadinged anymore 304 | * publishing fcu_untilted transform 305 | * fixed origin of GPS and RTK 306 | * fixed pass_rtk_as_odom 307 | * fixed gps origin 308 | * added missing [Odometry] to ROS prints 309 | * fixed measurement rotating bug 310 | * brick and vio altitude estimators 311 | * plane height estimator 312 | * using correct function for fusing tilts 313 | * fallback from BRICK cannot be BRICK 314 | * removed disambiguate brick heading 315 | * fixed inverse of tranform bug 316 | * new reference frames 317 | * fixed heading in odom_aux 318 | * Updated VIO covariances 319 | * fixed bug in angle unwrapping 320 | * optflow in body frame 321 | * rtk_local_odom now contains altitude above takeoff position 322 | * publishing uav_state msg 323 | * icp heading estimator in hector config 324 | * changed namespace from local_origin to uavX/local_origin 325 | * disable odometry callbacks before calling hover (safer) 326 | * udpated hector map reset routine (hover + disable_callbacks) 327 | * added missing parameters to uav config 328 | * brick unreliable when detections stop coming 329 | * updated brick topic 330 | * change drone frame to uav_name/fcu 331 | * implemented resetting routine for hector 332 | * icp estimation runing 333 | * New estimator based on ICP velocities 334 | * Contributors: Jan Bednar, Matej Petrlik, Matej Petrlik (desktop), Matouš Vrba, Pavel Petracek, Pavel Petráček, Petr Stepan, Tomas Baca, Vit Kratky, Vojtech Spurny 335 | 336 | 0.0.3 (2019-10-25) 337 | ------------------ 338 | * reset hector map after takeoff (tested in simulation) 339 | * checking hector velocity 340 | * hector reliable after switching estimator 341 | * hector reliability tuning 342 | * updated tracker_status topic to correct one 343 | * fusing zero tilts on the ground 344 | * odometry diag publishes availability of garmin (height_available) 345 | * publishing height (detilted and filtered garmin range) 346 | * publishing innovation 347 | * fixed switching to non-active estimator 348 | * fixed max_altitude = 0 349 | * fixed covariance Q vs R bug 350 | * fixed bug in correction 351 | * updated estimator list for gps in simulation 352 | * StateEstimator static Eigen matrices 353 | * added publishing of pose to rtk_republisher 354 | * fixed uninitialized variables 355 | * hopefully fixed vslam jump bug 356 | * vslam available in simulation 357 | * VSLAM PoseStamped -> PoseWithCovarianceStamped 358 | * vslam pose estimator 359 | * 2nd rehaul of launchfiles 360 | * rehauled launch files 361 | * deleted almost all launchfiles 362 | * fixed noise in velocity, preparing for vio in feedback 363 | * fixed uninitialized variables 364 | * fixed wrong hector corrections due to jumps in hector heading 365 | * fixed measurement for sonar 366 | * slow odom 1 hz 367 | * in hector we trust less 368 | * in hector we trust! 369 | * faster disturbance integration 370 | * Increased covariance of acceleration and velocity state 371 | * sonar enabled 372 | * increased covariance of sonar range 373 | * remap ultrasound 374 | * longer median fileter for sonar 375 | * added missing parameters for simulation 376 | * sonar added 377 | * finished state spam removed 378 | * removed terminal spam 379 | * fixed utm origin initial coordinates 380 | * zoh for hector pose 381 | * running estimators can be now specified in config files 382 | * utm_origin vs. local_origin is now decided based on takeoff estimator 383 | * added missing hector pose remap 384 | * brick estimator changes 385 | * Work in progress on brick estimator 386 | * Switching heading estimators now correctly rotates the lateral state 387 | * Fixed a bug in mavros velocity calculation - RTK should work again 388 | * added pixgarm if to odometry f550 launch file 389 | * child_frame_id problem when switching heading estimator 390 | * Contributors: Matej Petrlik, Matej Petrlik (desktop), Tomas Baca, UAV_44, Vojtech Spurny, uav43, uav5, uav61 391 | 392 | 0.0.2 (2019-07-01) 393 | ------------------ 394 | * Switching heading estimator rotates lateral states 395 | * Fixed max altitude 396 | * Moved support functions to separate file 397 | * + Brickflow estimator 398 | * + Hector estimator 399 | * Separate process covariance for optflow launch file 400 | * Slower disturbance acceleration integration 401 | * updated max optflow height 402 | * Detecting VIO failures 403 | * Tuned lateral GPS pos, vel covariances 404 | * Calling failsafe when no fallback odometry available 405 | * + monitor script 406 | * changed rinfo frequency of disturbance force 407 | * Fixed sign of target heading body rate from mavros 408 | * Printing disturbance force values to terminal 409 | * Simplified configs 410 | * Improved tilt fusion, disturbance acceleration 411 | * ICP median filter 412 | * Contributors: Matej Petrlik, Matej Petrlik (desktop), Matěj Petrlík, NAKI, Tomas Baca, Tomáš Báča, Vojtech Spurny, uav3, uav42, uav5, uav60 413 | 414 | 0.0.1 (2019-05-20) 415 | ------------------ 416 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1.2) 2 | project(mrs_uav_odometry) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | sensor_msgs 7 | geometry_msgs 8 | tf2_geometry_msgs 9 | std_msgs 10 | nav_msgs 11 | mrs_lib 12 | cmake_modules 13 | mrs_msgs 14 | nodelet 15 | dynamic_reconfigure 16 | ) 17 | 18 | find_package(mavros_msgs REQUIRED) 19 | 20 | set(CMAKE_CXX_STANDARD 17) 21 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 22 | 23 | # please, NEVER commit those alternative flags with specific overrides of optimization 24 | # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -Wall -O0 -g") 25 | 26 | # include Eigen3 27 | find_package(Eigen3 REQUIRED) 28 | set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS}) 29 | set(Eigen_LIBRARIES ${Eigen_LIBRARIES}) 30 | 31 | generate_dynamic_reconfigure_options( 32 | cfg/odometry_dynparam.cfg 33 | ) 34 | 35 | set(LIBRARIES 36 | Odometry RtkRepublisher 37 | ) 38 | 39 | catkin_package( 40 | INCLUDE_DIRS include 41 | LIBRARIES ${LIBRARIES} 42 | CATKIN_DEPENDS roscpp sensor_msgs std_msgs tf2_geometry_msgs geometry_msgs mrs_lib mrs_msgs 43 | DEPENDS mavros_msgs Eigen 44 | ) 45 | 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${mavros_msgs_INCLUDE_DIRS} 50 | ${Eigen_INCLUDE_DIRS} 51 | ) 52 | 53 | add_library(Odometry 54 | src/StateEstimator.cpp 55 | src/AltitudeEstimator.cpp 56 | src/AltitudeEstimatorAloamGarm.cpp 57 | src/HeadingEstimator.cpp 58 | src/odometry.cpp 59 | ) 60 | 61 | target_link_libraries(Odometry 62 | ${catkin_LIBRARIES} 63 | ${mavros_msgs_LIBRARIES} 64 | ${Eigen_LIBRARIES} 65 | ) 66 | 67 | add_dependencies(Odometry 68 | ${PROJECT_NAME}_gencfg 69 | ) 70 | 71 | add_library(RtkRepublisher 72 | src/rtk_republisher.cpp 73 | ) 74 | 75 | target_link_libraries(RtkRepublisher 76 | ${catkin_LIBRARIES} 77 | ${mavros_msgs_LIBRARIES} 78 | ) 79 | 80 | ## -------------------------------------------------------------- 81 | ## | Install | 82 | ## -------------------------------------------------------------- 83 | 84 | install(TARGETS ${LIBRARIES} 85 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 87 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 88 | ) 89 | 90 | install(DIRECTORY launch config rviz plot_juggler 91 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 92 | ) 93 | 94 | install(DIRECTORY scripts 95 | USE_SOURCE_PERMISSIONS 96 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 97 | ) 98 | 99 | install(DIRECTORY scripts/ 100 | USE_SOURCE_PERMISSIONS 101 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 102 | ) 103 | 104 | install(DIRECTORY include/ 105 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 106 | ) 107 | 108 | install(DIRECTORY ./ 109 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 110 | FILES_MATCHING PATTERN "*.xml" 111 | ) 112 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Multi-robot Systems (MRS) group at Czech Technical University in Prague 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | 3. 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MRS UAV Odometry 2 | 3 | ![](.fig/thumbnail.jpg) 4 | 5 | | Build status | [![Build Status](https://github.com/ctu-mrs/mrs_uav_odometry/workflows/Noetic/badge.svg)](https://github.com/ctu-mrs/mrs_uav_odometry/actions) | 6 | |--------------|------------------------------------------------------------------------------------------------------------------------------------------------| 7 | 8 | This package provides state estimation of the UAV dynamics based on sensor fusion of onboard sensors. 9 | The estimated states are: 10 | 11 | * lateral position 12 | * lateral velocity 13 | * lateral acceleration 14 | * heading 15 | * heading rate 16 | 17 | Within the [MRS UAV System](https://github.com/ctu-mrs/mrs_uav_system), the resulting state estimate is used by the [ControlManager](https://github.com/ctu-mrs/mrs_uav_managers) for feedback [control](https://github.com/ctu-mrs/mrs_uav_controllers) of the UAV. 18 | The provided *odometry diagnostics* topic is used by the [Gain/Constraint-Managers](https://github.com/ctu-mrs/mrs_uav_managers) for applying proper dynamics constraints and controller gains based on the current estimator type. 19 | 20 | ## Bank of estimators 21 | 22 | The odometry node maintains a bank of filters with independent hypotheses. 23 | Multiple sensors can be combined into a *sensor group*, which is input to one of the estimators. 24 | The estimators run independently and are multiplexed to obtain the *main* estimate. 25 | Only the selected *main* estimated is used for the feedback [control](https://github.com/ctu-mrs/mrs_uav_controllers). 26 | Here follows the list of selected estimators with their inputs. 27 | 28 | * "OPTFLOW" 29 | * horizontal: [optic flow](https://github.com/ctu-mrs/mrs_optic_flow) velocity 30 | * vertical: range finder height 31 | * heading: [optic flow](https://github.com/ctu-mrs/mrs_optic_flow) yaw rate 32 | * "GPS" 33 | * horizontal: GNSS position 34 | * vertical: range finder height 35 | * heading: magnetometer 36 | * "OPTFLOWGPS" 37 | * horizontal: GNSS position, [optic flow](https://github.com/ctu-mrs/mrs_optic_flow) velocity 38 | * vertical: range finder height 39 | * heading: magnetometer 40 | * "RTK" 41 | * horizontal: RTK GNSS position 42 | * vertical: range finder height 43 | * heading: magnetometer 44 | * "LIDAR" 45 | * all states: 3-D LIDAR position and heading 46 | * "VIO" 47 | * horizontal: Visual Inertial Odometry position 48 | * heading: Visual Inertial Odometry heading 49 | * vertical: range finder height 50 | * "HECTOR" 51 | * horizontal: Hector slam position 52 | * heading: Hector slam heading 53 | * vertical: range finder height 54 | 55 | ## Provided topics 56 | 57 | The *main* estimator output currently used for [control](https://github.com/ctu-mrs/mrs_uav_controllers) (in the custom *UavState* message type): 58 | ``` 59 | /uav*/odometry/uav_state 60 | ``` 61 | The *main* estimator output currently used for [control](https://github.com/ctu-mrs/mrs_uav_controllers) (in the custom *Odometry* message type): 62 | ``` 63 | /uav*/odometry/odom_main 64 | ``` 65 | Particular estimator topics: 66 | ``` 67 | /uav*/odometry/odom_optflow 68 | /uav*/odometry/odom_gps 69 | /uav*/odometry/odom_optflowgps 70 | /uav*/odometry/odom_rtk 71 | /uav*/odometry/odom_lidar 72 | /uav*/odometry/odom_vio 73 | /uav*/odometry/odom_hector 74 | ``` 75 | 76 | ## Switching current estimator 77 | 78 | ```bash 79 | rosservice call /uav*/odometry/change_odometry_source optflow 80 | ``` 81 | -------------------------------------------------------------------------------- /cfg/odometry_dynparam.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE='mrs_uav_odometry' 4 | import roslib; 5 | roslib.load_manifest(PACKAGE) 6 | 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator(); 10 | 11 | pos_cov = gen.add_group("Lateral Position Measurement Covariances"); 12 | 13 | pos_cov.add("R_pos_mavros", double_t, 1, "Mavros position", 1, 0.00001, 100000000); 14 | pos_cov.add("R_pos_vio", double_t, 2, "VIO position", 1, 0.00001, 100000000); 15 | pos_cov.add("R_pos_vslam", double_t, 2, "VSLAM position", 1, 0.00001, 100000000); 16 | pos_cov.add("R_pos_rtk", double_t, 8, "RTK position", 1, 0.00001, 100000000); 17 | pos_cov.add("R_pos_brick", double_t, 16, "Brick position", 1, 0.00001, 100000000); 18 | pos_cov.add("R_pos_hector", double_t, 16, "Hector position", 1, 0.00001, 100000000); 19 | pos_cov.add("R_pos_aloam", double_t, 16, "ALOAM position", 1, 0.00001, 100000000); 20 | pos_cov.add("R_pos_liosam", double_t, 16, "LIOSAM position", 1, 0.00001, 100000000); 21 | pos_cov.add("R_pos_uwb", double_t, 16, "UWB position", 1, 0.00001, 100000000); 22 | 23 | vel_cov = gen.add_group("Lateral Velocity Measurement Covariances"); 24 | vel_cov.add("R_vel_mavros", double_t, 32, "Mavros velocity", 1, 0.00001, 100000000); 25 | vel_cov.add("R_vel_vio", double_t, 64, "VIO velocity", 1, 0.00001, 100000000); 26 | vel_cov.add("R_vel_icp", double_t, 64, "ICP velocity", 1, 0.00001, 100000000); 27 | vel_cov.add("R_vel_optflow", double_t, 256, "Optflow velocity", 1, 0.00001, 100000000); 28 | vel_cov.add("R_vel_rtk", double_t, 256, "RTK velocity", 1, 0.00001, 100000000); 29 | vel_cov.add("R_vel_liosam", double_t, 256, "LIOSAM velocity", 1, 0.00001, 100000000); 30 | 31 | acc_cov = gen.add_group("Lateral Acceleration Measurement Covariances"); 32 | acc_cov.add("R_acc_imu_lat", double_t, 256, "IMU acceleration", 1, 0.00001, 100000000); 33 | 34 | proc_cov = gen.add_group("Lateral Process Covariances"); 35 | proc_cov.add("Q_lat_pos", double_t, 512, "Pose state", 1, 0.00001, 100000000); 36 | proc_cov.add("Q_lat_vel", double_t, 512, "Velocity state", 1, 0.00001, 100000000); 37 | proc_cov.add("Q_lat_acc", double_t, 1024, "Acceleration state", 1, 0.00001, 100000000); 38 | 39 | hdg_cov = gen.add_group("Heading Measurement Covariances"); 40 | hdg_cov.add("R_hdg_compass", double_t, 4096, "Compass hdg", 1, 0.00001, 100000000); 41 | hdg_cov.add("R_hdg_hector", double_t, 4096, "Hector hdg", 1, 0.00001, 100000000); 42 | hdg_cov.add("R_hdg_aloam", double_t, 4096, "ALOAM hdg", 1, 0.00001, 100000000); 43 | hdg_cov.add("R_hdg_liosam", double_t, 4096, "LIOSAM hdg", 1, 0.00001, 100000000); 44 | hdg_cov.add("R_hdg_brick", double_t, 4096, "Brick hdg", 1, 0.00001, 100000000); 45 | hdg_cov.add("R_hdg_vio", double_t, 4096, "VIO hdg", 1, 0.00001, 100000000); 46 | hdg_cov.add("R_hdg_vslam", double_t, 4096, "VSLAM hdg", 1, 0.00001, 100000000); 47 | hdg_cov.add("R_rate_gyro", double_t, 8192, "Gyro rate", 1, 0.00001, 100000000); 48 | hdg_cov.add("R_rate_optflow", double_t, 8192, "Optflow rate", 1, 0.00001, 100000000); 49 | hdg_cov.add("R_rate_icp", double_t, 8192, "ICP rate", 1, 0.00001, 100000000); 50 | hdg_cov.add("R_rate_liosam", double_t, 8192, "LIOSAM rate", 1, 0.00001, 100000000); 51 | 52 | alt_cov = gen.add_group("Altitude Measurement Covariances"); 53 | alt_cov.add("R_height_range", double_t, 8192, "Rangefinder height", 1, 0.00001, 100000000); 54 | alt_cov.add("R_height_plane", double_t, 8192, "Plane height", 1, 0.00001, 100000000); 55 | alt_cov.add("R_height_brick", double_t, 8192, "Brick height", 1, 0.00001, 100000000); 56 | alt_cov.add("R_height_vio", double_t, 8192, "VIO height", 1, 0.00001, 100000000); 57 | alt_cov.add("R_height_aloam", double_t, 8192, "ALOAM height", 1, 0.00001, 100000000); 58 | alt_cov.add("R_height_liosam", double_t, 8192, "LIOSAM height", 1, 0.00001, 100000000); 59 | alt_cov.add("R_height_baro", double_t, 8192, "BARO height", 1, 0.00001, 100000000); 60 | alt_cov.add("R_vel_baro", double_t, 8192, "Barometric velocity", 1, 0.00001, 100000000); 61 | alt_cov.add("R_vel_liosam_z", double_t, 8192, "LIOSAM z-axis velocity", 1, 0.00001, 100000000); 62 | alt_cov.add("R_acc_imu", double_t, 16384, "IMU accelerations", 1, 0.00001, 100000000); 63 | 64 | proc_cov = gen.add_group("Altitude Process Covariances"); 65 | proc_cov.add("Q_alt_pos", double_t, 512, "Pose state", 1, 0.00001, 100000000); 66 | proc_cov.add("Q_alt_vel", double_t, 512, "Velocity state", 1, 0.00001, 100000000); 67 | proc_cov.add("Q_alt_acc", double_t, 1024, "Acceleration state", 1, 0.00001, 100000000); 68 | 69 | proc_cov = gen.add_group("Altitude Input Coefficient"); 70 | proc_cov.add("alt_input_coeff", double_t, 512, "Input coefficient", 1, 0.00001, 100000000); 71 | 72 | exit(gen.generate(PACKAGE, "mrs_uav_odometry", "odometry_dynparam")) 73 | -------------------------------------------------------------------------------- /config/aloamgarm.yaml: -------------------------------------------------------------------------------- 1 | 2 | aloamgarm: 3 | debug: False # publish debug topics 4 | 5 | repredictor_buffer_size: 200 6 | 7 | # Median filter detecting rangefinder value changes 8 | mf_changes_buffer_size: 10 9 | mf_changes_max_diff: 0.1 10 | mf_changes_max_diff_close_to_ground: 0.7 # larger difference threshold for low heights 11 | mf_close_to_ground_threshold: 0.5 # max distance from ground where larger difference threshold is active 12 | # !!!!!! also decrease buffer_size for garmin median filter in odometry config to e.g. 10 samples !!!!!! 13 | 14 | eigenvalue_hysteresis_upper: 120 # SLAM considered reliable if eigenvalue rises above this 15 | eigenvalue_hysteresis_lower: 100 # SLAM considered unreliable if eigenvalue drops below this 16 | 17 | # Multiplication factors for process noise 18 | q_factor_range_bias_slam_ok: 1.0E+4 19 | q_factor_range_bias_range_jump: 1.0E+10 20 | q_factor_slam_bias: 1.0E+7 21 | 22 | # Multiplication factors for measurement noise 23 | r_factor_range_jump: 1.0E+5 24 | r_factor_slam_bad: 1.0E+3 25 | 26 | # Additional H matrices 27 | biased_state_count: 3 # Change when adding states 28 | H_range_biased: [1.0, 0, 0, -1.0, 0, 0] 29 | H_slam_biased: [1.0, 0, 0, 0, -1.0, 0] 30 | H_baro_biased: [0, 1.0, 0, 0, 0, 1.0] 31 | 32 | # Bias process noise 33 | q_biases: 1.0 34 | 35 | nis_buffer_size: 5 36 | nis_threshold: 3.0E-5 37 | nis_avg_threshold: 3.9E-5 38 | 39 | use_initial_conditions: False 40 | initial_state: [0.06469667308509643, 0.008255636646541047, -8.556493680058822e-06, -0.18643197312526824, 0.114846457567612, 0.41031111641590556] 41 | initial_cov: [814439600.7498282, 5517.468754612113, 0.6491906207423478, 814436178.2259641, 814438360.3419883, -158.31430963497, 5517.468752780861, 28003.10092377287, 54.10351141162824, 21.57755647239319, 1447.8610701337914, -203.9563292736764, 0.6491906208293747, 54.10351141162862, 1628.8552834575803, 0.0031853954265384046, -0.4081822835816807, -0.03156593163335334, 814436178.2259637, 21.577558317565458, 0.0031853953371403336, 814436166.0015874, 814436173.4927006, -37.25012355769622, 814438360.3419887, 1447.861071895208, -0.408182283663971, 814436173.4927008, 814527952.6211728, -113.83938082129612, -158.31430825760424, -203.95632927343576, -0.031565931633386383, -37.250122179762485, -113.83937944689912, 359.2772165592028] 42 | initial_time: 1628850637.0947604 43 | -------------------------------------------------------------------------------- /config/altitude_estimator.yaml: -------------------------------------------------------------------------------- 1 | altitude_estimators: 2 | 3 | # The available altitude estimators 4 | altitude_estimators: [ 5 | "HEIGHT", 6 | "PLANE", 7 | "BRICK", 8 | "VIO", 9 | "ALOAM", 10 | "ALOAMGARM", 11 | "ALOAMREP", 12 | "BARO", 13 | "RTK", 14 | "LIOSAM" 15 | ] 16 | 17 | # The measured states of the model 18 | model_states: [ 19 | "HEIGHT", 20 | "VELOCITY", 21 | "ACCELERATION" 22 | ] 23 | 24 | # The available measurements 25 | measurements: [ 26 | "height_range", 27 | "height_sonar", 28 | "height_plane", 29 | "height_brick", 30 | "height_vio", 31 | "height_aloam", 32 | "height_liosam", 33 | "height_baro", 34 | "height_rtk", 35 | "vel_baro", 36 | "vel_vio", 37 | "vel_liosam_z", 38 | "acc_imu" 39 | ] 40 | 41 | # The fused measurements for each state estimator 42 | fused_measurements: 43 | HEIGHT: ["height_range", "vel_baro"] 44 | PLANE: ["height_plane", "vel_baro"] 45 | BRICK: ["height_brick", "vel_baro"] 46 | VIO: ["height_vio", "vel_vio"] 47 | ALOAM: ["height_aloam", "vel_baro"] 48 | ALOAMREP: ["height_aloam", "vel_baro"] 49 | LIOSAM: ["height_liosam", "vel_liosam_z"] 50 | BARO: ["height_baro", "vel_baro"] 51 | ALOAMGARM: ["height_aloam", "height_range", "vel_baro"] 52 | RTK: ["height_rtk", "vel_baro"] 53 | 54 | # Mapping of measurements to states 55 | measurement_states: 56 | height_range: HEIGHT 57 | height_sonar: HEIGHT 58 | height_plane: HEIGHT 59 | height_brick: HEIGHT 60 | height_vio: HEIGHT 61 | height_aloam: HEIGHT 62 | height_liosam: HEIGHT 63 | height_baro: HEIGHT # z coordinate od mavros odometry (worse than integrated z twist) 64 | height_rtk: HEIGHT 65 | vel_baro: VELOCITY 66 | vel_vio: VELOCITY 67 | vel_liosam_z: VELOCITY 68 | acc_imu: ACCELERATION 69 | 70 | # Mapping states to model 71 | state_mapping: 72 | HEIGHT: [1.0, 0.0, 0.0] 73 | VELOCITY: [0.0, 1.0, 0.0] 74 | ACCELERATION: [0.0, 0.0, 1.0] 75 | 76 | # Untilted garmin filter (true height above terrain unsuitable for control) 77 | height: 78 | Q: [0.1] 79 | R: [0.01] 80 | -------------------------------------------------------------------------------- /config/debug_verbosity.yaml: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.mrs_uav_odometry=DEBUG 2 | -------------------------------------------------------------------------------- /config/default_config.yaml: -------------------------------------------------------------------------------- 1 | version: "1.0.4.0" 2 | 3 | # Estimators used during takeoff 4 | # Lateral state estimator: 5 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, VIO, VSLAM, HECTOR 6 | lateral_estimator: "GPS" 7 | 8 | # Altitude state estimator: 9 | # HEIGHT - rangefinder, PLANE - height above plane estimated from realsense, BRICK - height above BRICK 10 | altitude_estimator: "HEIGHT" 11 | 12 | # Heading state estimator: 13 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 14 | heading_estimator: "PIXHAWK" 15 | 16 | # Active estimators are started at node launch and can be switched to during flight 17 | state_estimators: 18 | active: ["OPTFLOW", "GPS", "RTK", "VIO", "VSLAM", "BRICK", "BRICKFLOW", "HECTOR", "ICP", "ALOAM", "ALOAMGARM", "ALOAMREP", "LIOSAM"] 19 | 20 | heading_estimators: 21 | active: ["PIXHAWK", "GYRO", "COMPASS", "OPTFLOW", "HECTOR", "BRICK", "VIO", "VSLAM", "ICP", "BRICKFLOW", "ALOAM", "ALOAMREP", "LIOSAM"] 22 | 23 | altitude_estimators: 24 | active: ["HEIGHT", "PLANE", "BRICK", "VIO", "ALOAM", "ALOAMGARM", "ALOAMREP", "BARO", "RTK", "LIOSAM"] 25 | 26 | # rates of publishers# #{ 27 | publish_rate: 28 | main: 100 # [Hz] main and aux odometry 29 | slow: 2 # [Hz] throttled main odometry 30 | diag: 10 # [Hz] diagnostics 31 | max_altitude: 10 # [Hz] maximum allowed altitude 32 | est_states: 10 # [Hz] inner states of estimators 33 | # # #} 34 | 35 | # debug parameters# #{ 36 | debug: 37 | publish_corrections: true # publish debug topics of measurement corrections 38 | publish_estimator_states: true # publish debug topics of measurement estimator states 39 | publish_fused_odom: true # otherwise publish unfused pixhawk odometry instead 40 | publish_pixhawk_velocity: false # pixhawk velocity instead of estimated velocity 41 | publish_servoing: false # publish diagnostics about visual servoing 42 | pass_rtk_as_odom: false # publish rtk instead of odometry dangerous outside simulation! 43 | # # #} 44 | 45 | # sensor z offset# #{ 46 | # (fallback when TF not available) 47 | offset: 48 | garmin: -0.03 # [m] 49 | sonar: -0.03 # [m] 50 | fcu_height: 0.2 # [m] (above ground) 51 | # # #} 52 | 53 | # altitude estimation parameters# #{ 54 | 55 | altitude: 56 | 57 | # Do not fuse rangefinder measurements when the UAV is tilted more than this angle 58 | excessive_tilt: deg(30) 59 | 60 | # Max value of correction when corrections are being saturated (typically after toggling garmin corrections on) 61 | max_saturated_correction: 0.001 # [m] 62 | 63 | # Altitude limits 64 | max_default: 30.0 # Used only when safety area is not used 65 | max_optflow: 10.0 # Do not allow switching to optflow above this altitude 66 | max_allowed_flight_height_garmin: 10.0 # Do not allow flying higher with garmin-based altitude estimator 67 | 68 | # Custom TF rules 69 | use_rtk_altitude: false # has to be true for correct TF when using RTK altitude 70 | use_garmin_for_vio_tf: false # has to be true for correct TF when using Garmin height with VIO lateral estimator 71 | 72 | # covariances# #{ 73 | 74 | # Process covariance 75 | Q: [100.0, 0, 0, 76 | 0, 100000.0, 0, 77 | 0, 0, 100000.0] 78 | 79 | # Covariances of measurements 80 | R: 81 | height_range: [100.0] 82 | height_sonar: [100.0] 83 | height_plane: [100.0] 84 | height_brick: [100.0] 85 | height_vio: [0.001] 86 | vel_vio: [0.01] 87 | height_aloam: [1.0] 88 | height_liosam: [10000.0] 89 | height_baro: [1.0] 90 | height_rtk: [100.0] 91 | vel_baro: [100.0] 92 | vel_liosam_z: [1000.0] 93 | acc_imu: [100.0] 94 | 95 | # # #} 96 | 97 | # median filters# #{ 98 | # Parameters of altitude median filters - buffer_size [samples], max_diff [m] (difference of input value from median to be considered valid) 99 | median_filter: 100 | 101 | garmin: 102 | buffer_size: 200 103 | max_diff: 2.0 104 | 105 | sonar: 106 | buffer_size: 200 107 | max_diff: 2.0 108 | 109 | plane: 110 | buffer_size: 40 111 | max_diff: 2.0 112 | 113 | brick: 114 | buffer_size: 200 115 | max_diff: 4.0 116 | 117 | vio: 118 | buffer_size: 200 119 | max_diff: 2.0 120 | 121 | aloam: 122 | buffer_size: 20 123 | max_diff: 2.0 124 | 125 | liosam: 126 | buffer_size: 20 127 | max_diff: 2.0 128 | 129 | # # #} 130 | 131 | # gates# #{ 132 | # Gate for min and max values of measurements [m] 133 | gate: 134 | garmin: 135 | min: 0.05 136 | max: 40 137 | use_inno_gate: true # innovation gate (declining large difference between state and measurement) 138 | inno_gate_value: 5.0 # [m] 139 | 140 | sonar: 141 | min: 0.1 142 | max: 4.0 143 | 144 | plane: 145 | min: 0.5 146 | max: 20.0 # uses garmin above ~5 m 147 | 148 | brick: 149 | min: 0.2 150 | max: 5.0 151 | 152 | vio: 153 | min: -100.0 154 | max: 100.0 155 | 156 | aloam: 157 | min: -1000.0 158 | max: 1000.0 159 | 160 | liosam: 161 | min: -1000.0 162 | max: 1000.0 163 | 164 | # # #} 165 | 166 | # # #} 167 | 168 | # lateral estimation parameters# #{ 169 | lateral: 170 | 171 | # covariances# #{ 172 | 173 | # Process covariance 174 | Q: [0.001, 0, 0, 175 | 0, 0.01, 0, 176 | 0, 0, 0.01] 177 | 178 | # Covariances of measurements 179 | R: 180 | vel_optflow: [1] 181 | pos_mavros: [0.01] 182 | vel_mavros: [100] 183 | tilt_mavros: [100] 184 | pos_vslam: [1] 185 | pos_vio: [0.01] #0.1 186 | vel_vio: [0.1] #10 187 | pos_object: [100] 188 | vel_object: [10000] 189 | pos_brick: [0.01] 190 | vel_brick: [100] 191 | pos_hector: [1] 192 | pos_tower: [1000] 193 | pos_aloam: [1] 194 | pos_liosam: [10] 195 | pos_uwb: [1] 196 | vel_liosam: [100] 197 | vel_icp: [10] 198 | pos_rtk: [0.001] 199 | vel_rtk: [1] 200 | acc_imu: [1000000000] # turned off in estimators 201 | 202 | # RTK estimator must have a different model 203 | rtk: 204 | 205 | A: [1, 0, 206 | 0, 1] 207 | 208 | B: [0.01, 0, 209 | 0, 0.01] 210 | 211 | H: [1, 0, 212 | 0, 1] 213 | 214 | Q: [0.01, 0, 215 | 0, 0.01] 216 | 217 | R: [1, 0, 218 | 0, 1] 219 | 220 | P: [1, 0, 221 | 0, 1] 222 | 223 | # # #} 224 | 225 | # median filters# #{ 226 | # Parameters of lateral median filters - buffer_size [samples], max_diff [m/s] (difference of input value from median to be considered valid) 227 | median_filter: 228 | 229 | optflow: 230 | use: true 231 | buffer_size: 10 232 | max_diff: 2.0 233 | 234 | icp: 235 | use: true 236 | buffer_size: 20 237 | max_diff: 2.0 238 | 239 | # # #} 240 | 241 | # gates# #{ 242 | # Gate for max absolute twist values of measurements in each axis (x, y) [m/s] 243 | gate: 244 | 245 | optflow: 246 | max: 5.0 247 | 248 | icp: 249 | max: 5.0 250 | 251 | # # #} 252 | 253 | # correction saturation # #{ 254 | saturate_mavros_position: false 255 | max_mavros_pos_correction: 0.5 256 | max_vio_pos_correction: 0.5 257 | max_object_pos_correction: 0.5 258 | max_brick_pos_correction: 1.0 259 | max_rtk_pos_correction: 0.5 260 | max_vslam_pos_correction: 0.2 261 | max_t265_vel: 10.0 262 | max_safe_brick_jump: 0.3 263 | 264 | # # #} 265 | 266 | # gps fallback# #{ 267 | gps_fallback: 268 | allowed: false 269 | fallback_estimator: "OPTFLOW" 270 | cov_limit: 10.0 # limit covariance in lateral axes to toggle fallback 271 | bad_samples: 300 # samples with large covariance before fallback (100 samples ~ 1 second) 272 | altitude: 4.0 # go to altitude before fallback (optflow should work up to 10 m) 273 | altitude_wait_time: 5.0 # [s] timeout for reaching target altitude 274 | return_after_ok: true # return to GPS when covariance is back to normal 275 | cov_ok: 6.0 # threshold covariance for return from fallback 276 | good_samples: 300 # samples with normal covariance before returning from fallback (100 samples ~ 1 second) 277 | 278 | # # #} 279 | 280 | # optflow# #{ 281 | optflow: 282 | optimized_low: false # use optimized optflow for takeoff and landing 283 | dynamic_cov: false # use dynamic measurement covariance estimated based on the altitude 284 | dynamic_cov_scale: 1 # scale the estimated covariance by this factor 285 | 286 | # # #} 287 | 288 | # hector# #{ 289 | hector: 290 | reset_after_takeoff: false # Gets rid of map clutter accumulated during takeoff tilting 291 | reset_routine: false # Switches to ICP or OPTFLOW, stores offset, resets map, switches back to HECTOR 292 | 293 | # # #} 294 | 295 | # brick# #{ 296 | brick: 297 | timeout: 0.5 # [s] timeout until fallback from BRICK after loosing detections# #} 298 | 299 | # slam# #{ 300 | slam: 301 | use_general_slam_origin: true # use slam_origin instead of aloam_origin/liosam_origin 302 | ouster_scan_delay: 0.0 # [seconds] 303 | # # #} 304 | 305 | # rtk # #{ 306 | use_full_rtk: true # Full RTK for precise and reliable RTK stations only 307 | rtk_fuse_sps: true # Fuse RTK even without basestation corrections 308 | # # #} 309 | 310 | # # #} 311 | 312 | # heading estimation parameters# #{ 313 | heading: 314 | 315 | gyro_fallback: true 316 | 317 | # covariances# #{ 318 | 319 | # Process covariance 320 | Q: [0.1, 0, 0, 321 | 0, 0.1, 0, 322 | 0, 0, 0.001] 323 | 324 | # Covariances of measurements 325 | R: 326 | hdg_compass: [0.1] 327 | hdg_hector: [0.01] 328 | hdg_tower: [0.01] 329 | hdg_aloam: [0.0001] 330 | hdg_liosam: [10] 331 | hdg_brick: [10] 332 | hdg_vio: [100] #100 333 | hdg_vslam: [1] 334 | rate_gyro: [0.1] 335 | rate_optflow: [10] 336 | rate_icp: [10] 337 | rate_liosam: [1] 338 | # # #} 339 | 340 | # median filters# #{ 341 | # Parameters of heading median filters - buffer_size [samples], max_diff [m] (difference of input value from median to be considered valid) 342 | median_filter: 343 | 344 | optflow: 345 | use: true 346 | buffer_size: 100 347 | max_diff: 2.0 348 | 349 | icp: 350 | use: true 351 | buffer_size: 100 352 | max_diff: 2.0 353 | 354 | # # #} 355 | 356 | # gates# #{ 357 | # Gate for max absolute hdg rate values of measurements [rad/s] 358 | gate: 359 | 360 | optflow: 361 | max: 2.0 362 | 363 | icp: 364 | max: 2.0 365 | 366 | # # #} 367 | 368 | # correction saturation # #{ 369 | max_brick_hdg_correction: deg(5) 370 | accum_hdg_brick_alpha: 0.1 371 | max_safe_brick_hdg_jump: deg(20) 372 | 373 | # # #} 374 | 375 | # # #} 376 | 377 | # Time between mainTimer ticks to be considered as hiccup 378 | hiccup_time_threshold: 0.03 # [s] 379 | 380 | scope_timer: 381 | 382 | enabled: false 383 | log_filename: "" # if empty, scope timers output to terminal; otherwise log to file 384 | -------------------------------------------------------------------------------- /config/heading_estimator.yaml: -------------------------------------------------------------------------------- 1 | heading_estimators: 2 | 3 | # The available altitude estimators 4 | heading_estimators: [ 5 | "PIXHAWK", 6 | "GYRO", 7 | "COMPASS", 8 | "OPTFLOW", 9 | "HECTOR", 10 | "BRICK", 11 | "VIO", 12 | "VSLAM", 13 | "ICP", 14 | "BRICKFLOW", 15 | "ALOAM", 16 | "ALOAMREP", 17 | "LIOSAM", 18 | ] 19 | 20 | # The measured states of the model 21 | model_states: [ 22 | "HDG", 23 | "HDG_RATE", 24 | "HDG_ACC" 25 | ] 26 | 27 | # The available measurements 28 | measurements: [ 29 | "hdg_compass", 30 | "rate_gyro", 31 | "rate_optflow", 32 | "rate_liosam", 33 | "hdg_hector", 34 | "hdg_brick", 35 | "hdg_vio", 36 | "hdg_vslam", 37 | "rate_icp", 38 | "hdg_aloam", 39 | "hdg_liosam", 40 | ] 41 | 42 | # The fused measurements for each state estimator 43 | fused_measurements: 44 | PIXHAWK: [] 45 | GYRO: ["rate_gyro"] 46 | COMPASS: ["rate_gyro", "hdg_compass"] 47 | OPTFLOW: ["rate_gyro", "rate_optflow"] 48 | HECTOR: ["rate_gyro", "hdg_hector"] 49 | BRICK: ["rate_gyro", "hdg_brick"] 50 | VIO: ["rate_gyro", "hdg_vio"] 51 | VSLAM: ["rate_gyro", "hdg_vslam"] 52 | ICP: ["rate_icp"] 53 | BRICKFLOW: ["rate_gyro", "hdg_brick"] 54 | ALOAM: ["rate_gyro", "hdg_aloam", "rate_icp"] 55 | ALOAMREP: ["rate_gyro", "hdg_aloam", "rate_icp"] 56 | LIOSAM: ["hdg_liosam", "rate_liosam"] 57 | 58 | # Mapping of measurements to states 59 | measurement_states: 60 | hdg_compass: HDG 61 | rate_gyro: HDG_RATE 62 | rate_optflow: HDG_RATE 63 | rate_liosam: HDG_RATE 64 | hdg_hector: HDG 65 | hdg_brick: HDG 66 | hdg_vio: HDG 67 | hdg_vslam: HDG 68 | rate_icp: HDG_RATE 69 | hdg_aloam: HDG 70 | hdg_liosam: HDG 71 | 72 | # Mapping states to model 73 | state_mapping: 74 | HDG: [1.0, 0.0, 0.0] 75 | HDG_RATE: [0.0, 1.0, 0.0] 76 | HDG_ACC: [0.0, 0.0, 1.0] 77 | 78 | -------------------------------------------------------------------------------- /config/init_pose/init_pose.csv: -------------------------------------------------------------------------------- 1 | 1, 0.0, 0.0, 0.5, 0.0 2 | -------------------------------------------------------------------------------- /config/rtk_republisher.yaml: -------------------------------------------------------------------------------- 1 | rate: 100 # [Hz] 2 | 3 | offset_x: 465710.759073 4 | offset_y: 5249465.47126 5 | -------------------------------------------------------------------------------- /config/simulation/aloam.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "ALOAMREP" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "ALOAMREP" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "ALOAMREP" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["ALOAMREP"] 17 | 18 | altitude_estimators: 19 | active: ["ALOAMREP", "BARO", "HEIGHT"] 20 | 21 | publish_tf: 22 | ALOAM: true 23 | 24 | heading_estimators: 25 | active: ["PIXHAWK", "GYRO", "ALOAMREP"] 26 | 27 | lateral: 28 | slam: 29 | use_general_slam_origin: true # use slam_origin instead of aloam_origin/liosam_origin 30 | -------------------------------------------------------------------------------- /config/simulation/brick.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "GPS" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "PIXHAWK" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["GPS", "BRICK"] 17 | 18 | heading_estimators: 19 | active: ["PIXHAWK", "GYRO", "BRICK"] 20 | 21 | altitude_estimators: 22 | active: ["BARO", "HEIGHT", "BRICK"] 23 | -------------------------------------------------------------------------------- /config/simulation/brickflow.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "GPS" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "PIXHAWK" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["GPS", "OPTFLOW", "BRICKFLOW", "BRICK"] 17 | 18 | heading_estimators: 19 | active: ["PIXHAWK", "GYRO", "OPTFLOW", "BRICKFLOW", "BRICK"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BRICK", "BARO"] 23 | 24 | 25 | -------------------------------------------------------------------------------- /config/simulation/darpa.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "ALOAM" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "ALOAM" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "ALOAM" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["ALOAM"] 17 | 18 | heading_estimators: 19 | active: ["GYRO", "ALOAM"] 20 | 21 | altitude_estimators: 22 | active: ["ALOAM", "BARO"] 23 | 24 | lateral: 25 | # Covariances of measurements 26 | R: 27 | pos_aloam: [10] 28 | 29 | heading: 30 | # Covariances of measurements 31 | R: 32 | hdg_aloam: [0.1] 33 | 34 | altitude: 35 | R: 36 | height_aloam: [1] 37 | 38 | median_filter: 39 | garmin: 40 | buffer_size: 200 41 | max_diff: 2.0 42 | 43 | offset: 44 | garmin: -0.01 # [m] 45 | fcu_height: 0.0 # [m] 46 | -------------------------------------------------------------------------------- /config/simulation/gps.yaml: -------------------------------------------------------------------------------- 1 | # Lateral state estimator: 2 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 3 | lateral_estimator: "GPS" 4 | 5 | # Altitude state estimator: 6 | # HEIGHT - rangefinder 7 | altitude_estimator: "HEIGHT" 8 | 9 | # Heading state estimator: 10 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 11 | heading_estimator: "PIXHAWK" 12 | 13 | # Active estimators are started at node launch and can be switched to during flight 14 | state_estimators: 15 | active: ["GPS"] 16 | 17 | heading_estimators: 18 | active: ["PIXHAWK"] 19 | 20 | altitude_estimators: 21 | active: ["HEIGHT", "BARO"] 22 | -------------------------------------------------------------------------------- /config/simulation/hector.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "HECTOR" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "HECTOR" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["OPTFLOW", "HECTOR", "ICP"] 17 | 18 | heading_estimators: 19 | active: ["GYRO", "OPTFLOW", "HECTOR", "ICP"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO"] 23 | 24 | -------------------------------------------------------------------------------- /config/simulation/icp.yaml: -------------------------------------------------------------------------------- 1 | # Lateral state estimator: 2 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 3 | lateral_estimator: "ICP" 4 | 5 | # Altitude state estimator: 6 | # HEIGHT - rangefinder 7 | altitude_estimator: "HEIGHT" 8 | 9 | # Heading state estimator: 10 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 11 | heading_estimator: "GYRO" 12 | 13 | # Active estimators are started at node launch and can be switched to during flight 14 | state_estimators: 15 | active: ["GPS", "ICP"] 16 | 17 | heading_estimators: 18 | active: ["PIXHAWK", "GYRO", "ICP"] 19 | 20 | altitude_estimators: 21 | active: ["HEIGHT", "BARO"] 22 | -------------------------------------------------------------------------------- /config/simulation/liosam.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "LIOSAM" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "LIOSAM" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "LIOSAM" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["LIOSAM"] 17 | 18 | publish_tf: 19 | LIOSAM: false 20 | 21 | heading_estimators: 22 | active: ["GYRO", "LIOSAM"] 23 | 24 | altitude_estimators: 25 | active: ["HEIGHT", "BARO", "LIOSAM"] 26 | 27 | lateral: 28 | # Covariances of measurements 29 | R: 30 | pos_liosam: [10] 31 | vel_liosam: [10] 32 | 33 | heading: 34 | # Covariances of measurements 35 | R: 36 | hdg_liosam: [0.1] 37 | rate_liosam: [10] 38 | 39 | altitude: 40 | R: 41 | height_liosam: [1] 42 | vel_liosam_z: [100.0] 43 | 44 | -------------------------------------------------------------------------------- /config/simulation/optflow.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "OPTFLOW" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "OPTFLOW" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["GPS", "OPTFLOW", "BRICKFLOW"] 17 | 18 | heading_estimators: 19 | active: ["PIXHAWK", "GYRO", "OPTFLOW", "BRICKFLOW"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO"] 23 | -------------------------------------------------------------------------------- /config/simulation/rtk.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "RTK" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "RTK" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "PIXHAWK" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["GPS", "RTK"] 17 | 18 | heading_estimators: 19 | active: ["PIXHAWK", "GYRO", "COMPASS"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO", "RTK"] 23 | -------------------------------------------------------------------------------- /config/simulation/simulation_default.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, LIDAR, VIO, VSLAM, HECTOR 4 | lateral_estimator: "GPS" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder, PLANE - height above plane estimated from realsense 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "PIXHAWK" 13 | 14 | lateral: 15 | gps_fallback: 16 | allowed: true 17 | fallback_estimator: "OPTFLOW" 18 | cov_limit: 10.0 # limit covariance in lateral axes to toggle fallback 19 | bad_samples: 300 # samples with large covariance before fallback (100 samples ~ 1 second) 20 | altitude: 4.0 # go to altitude before fallback (optflow should work up to 10 m) 21 | return_after_ok: true # return to GPS when covariance is back to normal 22 | cov_ok: 6.0 # threshold covariance for return from fallback 23 | good_samples: 300 # samples with normal covariance before returning from fallback (100 samples ~ 1 second) 24 | -------------------------------------------------------------------------------- /config/simulation/vio.yaml: -------------------------------------------------------------------------------- 1 | # Lateral state estimator: 2 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 3 | lateral_estimator: "VIO" 4 | 5 | # Altitude state estimator: 6 | # HEIGHT - rangefinder 7 | altitude_estimator: "HEIGHT" 8 | 9 | # Heading state estimator: 10 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 11 | heading_estimator: "VIO" 12 | 13 | # Active estimators are started at node launch and can be switched to during flight 14 | state_estimators: 15 | active: ["GPS", "VIO"] 16 | 17 | heading_estimators: 18 | active: ["PIXHAWK", "GYRO", "VIO"] 19 | 20 | altitude_estimators: 21 | active: ["HEIGHT", "VIO", "BARO"] 22 | -------------------------------------------------------------------------------- /config/simulation/vslam.yaml: -------------------------------------------------------------------------------- 1 | # Lateral state estimator: 2 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 3 | lateral_estimator: "VSLAM" 4 | 5 | # Altitude state estimator: 6 | # HEIGHT - rangefinder 7 | altitude_estimator: "HEIGHT" 8 | 9 | # Heading state estimator: 10 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 11 | heading_estimator: "VSLAM" 12 | 13 | # Active estimators are started at node launch and can be switched to during flight 14 | state_estimators: 15 | active: ["GPS", "VSLAM"] 16 | 17 | heading_estimators: 18 | active: ["PIXHAWK", "GYRO", "VSLAM"] 19 | 20 | altitude_estimators: 21 | active: ["HEIGHT", "VIO", "BARO"] 22 | -------------------------------------------------------------------------------- /config/state_estimators.yaml: -------------------------------------------------------------------------------- 1 | state_estimators: 2 | 3 | # The number of model states 4 | n_model_states: 3 5 | 6 | # The available state estimators 7 | state_estimators: [ 8 | "OPTFLOW", 9 | "GPS", 10 | "OPTFLOWGPS", 11 | "RTK", 12 | "VIO", 13 | "BRICK", 14 | "T265", 15 | "HECTOR", 16 | "BRICKFLOW", 17 | "VSLAM", 18 | "ICP", 19 | "ALOAM", 20 | "ALOAMGARM", 21 | "ALOAMREP", 22 | "LIOSAM", 23 | "UWB", 24 | ] 25 | 26 | # The measured states of the model 27 | model_states: [ 28 | "POSITION", 29 | "VELOCITY", 30 | "ACCELERATION", 31 | ] 32 | 33 | # The available measurements 34 | measurements: [ 35 | "vel_optflow", 36 | "pos_mavros", 37 | "vel_mavros", 38 | "pos_rtk", 39 | "vel_rtk", 40 | "pos_vio", 41 | "vel_vio", 42 | "pos_brick", 43 | "vel_brick", 44 | "pos_hector", 45 | "pos_vslam", 46 | "vel_icp", 47 | "pos_aloam", 48 | "pos_liosam", 49 | "vel_liosam", 50 | "pos_uwb", 51 | "acc_imu", 52 | ] 53 | 54 | # The fused measurements for each state estimator 55 | fused_measurements: 56 | OPTFLOW: ["vel_optflow"] 57 | GPS: ["pos_mavros"] 58 | OPTFLOWGPS: ["vel_optflow", "pos_mavros"] 59 | RTK: ["pos_rtk", "vel_mavros"] 60 | HECTOR: ["pos_hector"] 61 | VIO: ["pos_vio", "vel_vio"] 62 | VSLAM: ["pos_vslam"] 63 | BRICK: ["pos_brick"] 64 | BRICKFLOW: ["vel_optflow", "pos_brick"] 65 | T265: [] 66 | ICP: ["vel_icp"] 67 | ALOAM: ["pos_aloam", "vel_icp"] 68 | ALOAMGARM: ["pos_aloam", "vel_icp"] 69 | ALOAMREP: ["pos_aloam", "vel_icp"] 70 | LIOSAM: ["pos_liosam", "vel_liosam"] 71 | UWB: ["pos_uwb"] 72 | 73 | # Mapping of measurements to states 74 | measurement_states: 75 | vel_optflow: VELOCITY 76 | pos_mavros: POSITION 77 | vel_mavros: VELOCITY 78 | pos_rtk: POSITION 79 | vel_rtk: VELOCITY 80 | pos_vio: POSITION 81 | vel_vio: VELOCITY 82 | pos_brick: POSITION 83 | vel_brick: VELOCITY 84 | pos_hector: POSITION 85 | pos_vslam: POSITION 86 | vel_icp: VELOCITY 87 | pos_aloam: POSITION 88 | pos_liosam: POSITION 89 | vel_liosam: VELOCITY 90 | pos_uwb: POSITION 91 | acc_imu: ACCELERATION 92 | 93 | # Mapping states to model 94 | state_mapping: 95 | POSITION: [1.0, 0.0, 0.0] 96 | VELOCITY: [0.0, 1.0, 0.0] 97 | ACCELERATION: [0.0, 0.0, 1.0] 98 | -------------------------------------------------------------------------------- /config/uav/aloam.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "ALOAMREP" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "ALOAMREP" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "ALOAMREP" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["ALOAMREP"] 17 | 18 | heading_estimators: 19 | active: ["GYRO", "ALOAMREP"] 20 | 21 | altitude_estimators: 22 | active: ["ALOAMREP", "BARO", "HEIGHT"] 23 | 24 | lateral: 25 | slam: 26 | use_general_slam_origin: true # use slam_origin instead of aloam_origin/liosam_origin 27 | 28 | # Process covariance 29 | Q: [1, 0, 0, 30 | 0, 1, 0, 31 | 0, 0, 1] 32 | 33 | # Covariances of measurements 34 | R: 35 | pos_aloam: [1] 36 | -------------------------------------------------------------------------------- /config/uav/darpa.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "ALOAM" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "ALOAM" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "ALOAM" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["ALOAM"] 17 | 18 | heading_estimators: 19 | active: ["GYRO", "ALOAM"] 20 | 21 | altitude_estimators: 22 | active: ["ALOAM", "BARO"] 23 | 24 | lateral: 25 | # Covariances of measurements 26 | R: 27 | pos_aloam: [10] 28 | 29 | heading: 30 | # Covariances of measurements 31 | R: 32 | hdg_aloam: [0.1] 33 | 34 | altitude: 35 | R: 36 | height_aloam: [1] 37 | 38 | median_filter: 39 | aloam: 40 | buffer_size: 200 41 | max_diff: 2.0 42 | 43 | offset: 44 | garmin: -0.01 # [m] 45 | fcu_height: 0.0 # [m] 46 | -------------------------------------------------------------------------------- /config/uav/gps.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "GPS" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "PIXHAWK" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["GPS"] 17 | 18 | heading_estimators: 19 | active: ["PIXHAWK", "GYRO", "COMPASS"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO"] 23 | -------------------------------------------------------------------------------- /config/uav/hector.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "HECTOR" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "HECTOR" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["OPTFLOW", "HECTOR"] 17 | 18 | heading_estimators: 19 | active: ["GYRO", "OPTFLOW", "HECTOR"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO"] 23 | 24 | lateral: 25 | R: 26 | vel_optflow: [1] 27 | 28 | -------------------------------------------------------------------------------- /config/uav/liosam.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "LIOSAM" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "LIOSAM" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "LIOSAM" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["LIOSAM"] 17 | 18 | publish_tf: 19 | LIOSAM: false 20 | 21 | heading_estimators: 22 | active: ["GYRO", "LIOSAM"] 23 | 24 | altitude_estimators: 25 | active: ["HEIGHT", "BARO", "LIOSAM"] 26 | 27 | lateral: 28 | slam: 29 | use_general_slam_origin: true # use slam_origin instead of aloam_origin/liosam_origin 30 | 31 | # Covariances of measurements 32 | R: 33 | pos_liosam: [10] 34 | vel_liosam: [10] 35 | 36 | heading: 37 | # Covariances of measurements 38 | R: 39 | hdg_liosam: [0.1] 40 | rate_liosam: [10] 41 | 42 | altitude: 43 | R: 44 | height_liosam: [10000] 45 | vel_liosam_z: [1000.0] 46 | 47 | -------------------------------------------------------------------------------- /config/uav/optflow.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "OPTFLOW" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "OPTFLOW" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["OPTFLOW"] 17 | 18 | heading_estimators: 19 | active: ["GYRO", "OPTFLOW"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO"] 23 | -------------------------------------------------------------------------------- /config/uav/rtk.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "RTK" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "RTK" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "PIXHAWK" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["GPS", "RTK"] 17 | 18 | heading_estimators: 19 | active: ["PIXHAWK", "GYRO"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO", "RTK"] 23 | 24 | lateral: 25 | R: 26 | vel_mavros: [0.1] 27 | 28 | altitude: 29 | 30 | # Use RTK altitude 31 | use_rtk_altitude: true # has to be true for correct TF when using RTK altitude 32 | 33 | R: 34 | height_rtk: [1.0] 35 | vel_baro: [100] 36 | -------------------------------------------------------------------------------- /config/uav/uav_default.yaml: -------------------------------------------------------------------------------- 1 | lateral: 2 | 3 | optflow_median_filter: true 4 | optflow_filter_buffer_size: 50 5 | optflow_filter_max_valid: 5.0 6 | optflow_filter_max_diff: 1.0 7 | 8 | gps_fallback: 9 | allowed: false 10 | fallback_estimator: "OPTFLOW" 11 | cov_limit: 10.0 # limit covariance in lateral axes to toggle fallback 12 | bad_samples: 300 # samples with large covariance before fallback (100 samples ~ 1 second) 13 | altitude: 4.0 # go to altitude before fallback (optflow should work up to 10 m) 14 | return_after_ok: true # return to GPS when covariance is back to normal 15 | cov_ok: 6.0 # threshold covariance for return from fallback 16 | good_samples: 300 # samples with normal covariance before returning from fallback (100 samples ~ 1 second) 17 | 18 | # Covariances of measurements 19 | R: 20 | vel_optflow: [0.1] 21 | 22 | heading: 23 | 24 | # Covariances of measurements 25 | R: 26 | hdg_compass: [0.1] 27 | hdg_hector: [0.01] 28 | hdg_brick: [1] 29 | hdg_vio: [1] 30 | hdg_vslam: [1] 31 | hdg_lidar: [1] 32 | rate_gyro: [0.1] 33 | rate_optflow: [10] 34 | rate_icp: [1] 35 | -------------------------------------------------------------------------------- /config/uav/uwb.yaml: -------------------------------------------------------------------------------- 1 | # Estimators used during takeoff 2 | # Lateral state estimator: 3 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 4 | lateral_estimator: "UWB" 5 | 6 | # Altitude state estimator: 7 | # HEIGHT - rangefinder 8 | altitude_estimator: "HEIGHT" 9 | 10 | # Heading state estimator: 11 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 12 | heading_estimator: "PIXHAWK" 13 | 14 | # Active estimators are started at node launch and can be switched to during flight 15 | state_estimators: 16 | active: ["UWB", "GPS", "RTK"] 17 | 18 | heading_estimators: 19 | active: ["PIXHAWK", "GYRO"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "BARO", "RTK"] 23 | 24 | lateral: 25 | R: 26 | vel_mavros: [0.1] 27 | 28 | altitude: 29 | 30 | # Use RTK altitude 31 | use_rtk_altitude: false # has to be true for correct TF when using RTK altitude 32 | 33 | R: 34 | height_rtk: [1.0] 35 | vel_baro: [100] 36 | -------------------------------------------------------------------------------- /config/uav/vio.yaml: -------------------------------------------------------------------------------- 1 | # Lateral state estimator: 2 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 3 | lateral_estimator: "VIO" 4 | 5 | # Altitude state estimator: 6 | # HEIGHT - rangefinder 7 | altitude_estimator: "VIO" 8 | 9 | # Heading state estimator: 10 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 11 | heading_estimator: "VIO" 12 | 13 | # Active estimators are started at node launch and can be switched to during flight 14 | state_estimators: 15 | active: ["VIO"] 16 | 17 | heading_estimators: 18 | # active: ["PIXHAWK", "GYRO", "VIO"] 19 | active: ["VIO"] 20 | 21 | altitude_estimators: 22 | active: ["HEIGHT", "VIO", "BARO"] 23 | -------------------------------------------------------------------------------- /config/uav/vio_testing.yaml: -------------------------------------------------------------------------------- 1 | # Lateral state estimator: 2 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 3 | lateral_estimator: "RTK" 4 | 5 | # Altitude state estimator: 6 | # HEIGHT - rangefinder 7 | altitude_estimator: "HEIGHT" 8 | 9 | # Heading state estimator: 10 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 11 | heading_estimator: "PIXHAWK" 12 | 13 | # Active estimators are started at node launch and can be switched to during flight 14 | state_estimators: 15 | active: ["GPS", "RTK", "VIO"] 16 | 17 | heading_estimators: 18 | active: ["PIXHAWK", "GYRO", "VIO"] 19 | 20 | altitude_estimators: 21 | active: ["HEIGHT", "BARO", "RTK", "VIO"] 22 | -------------------------------------------------------------------------------- /config/uav/vslam.yaml: -------------------------------------------------------------------------------- 1 | # Lateral state estimator: 2 | # OPTFLOW, GPS, OPTFLOWGPS, RTK, ICP, VIO, HECTOR 3 | lateral_estimator: "VSLAM" 4 | 5 | # Altitude state estimator: 6 | # HEIGHT - rangefinder 7 | altitude_estimator: "HEIGHT" 8 | 9 | # Heading state estimator: 10 | # GYRO - gyro, COMPASS - gyro, compass, OPTFLOW - gyro, optflow, HECTOR - gyro, hector slam 11 | heading_estimator: "VSLAM" 12 | 13 | # Active estimators are started at node launch and can be switched to during flight 14 | state_estimators: 15 | active: ["GPS", "VSLAM"] 16 | 17 | heading_estimators: 18 | active: ["PIXHAWK", "GYRO", "VSLAM"] 19 | -------------------------------------------------------------------------------- /include/AltitudeEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef ALTITUDE_ESTIMATOR_H 2 | #define ALTITUDE_ESTIMATOR_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "types.h" 19 | 20 | #define ALT_DT 0.01 21 | #define ALT_INPUT_COEFF 0.10 22 | 23 | namespace mrs_uav_odometry 24 | { 25 | 26 | 27 | class AltitudeEstimator { 28 | 29 | public: 30 | AltitudeEstimator(); 31 | AltitudeEstimator(const std::string &estimator_name, const std::vector &fusing_measurement, const std::vector &H_multi, const alt_Q_t &Q, 32 | const std::vector &R_multi, const bool use_repredictor = false); 33 | 34 | virtual bool doPrediction(const double input, const double dt, const ros::Time &input_stamp = ros::Time::now(), const ros::Time &predict_stamp = ros::Time::now()); 35 | virtual bool doPrediction(const double input, const ros::Time &input_stamp = ros::Time::now(), const ros::Time &predict_stamp = ros::Time::now()); 36 | virtual bool doCorrection(const double &measurement, int measurement_type, const ros::Time &meas_stamp = ros::Time::now(), 37 | const ros::Time &predict_stamp = ros::Time::now(), const std::string &measurement_name = std::string()); 38 | 39 | virtual bool getStates(alt_x_t &x); 40 | virtual bool getState(int state_id, double &state_val); 41 | virtual std::string getName(void); 42 | virtual bool setState(int state_id, const double &state_val); 43 | virtual bool setR(double R, int measurement_type); 44 | virtual bool getR(double &R, int measurement_type); 45 | virtual bool setQ(double cov, const Eigen::Vector2i &idx); 46 | virtual bool getQ(double &cov, const Eigen::Vector2i &idx); 47 | virtual bool setInputCoeff(double coeff); 48 | virtual bool getCovariance(alt_P_t &P); 49 | virtual bool setCovariance(const alt_P_t &P); 50 | virtual bool reset(const alt_x_t &states); 51 | 52 | private: 53 | std::string m_estimator_name; 54 | std::vector m_fusing_measurement; 55 | int m_n_states; 56 | size_t m_n_measurement_types; 57 | 58 | // repredictor buffer size 59 | const unsigned m_buf_sz = 200; 60 | 61 | // repredictor 62 | std::unique_ptr mp_rep; 63 | 64 | // State transition matrix 65 | alt_A_t m_A; 66 | 67 | // Input matrix 68 | alt_B_t m_B; 69 | 70 | // Input coefficient 71 | double m_b = ALT_INPUT_COEFF; 72 | 73 | // Array with mapping matrices for each fused measurement 74 | std::vector m_H_multi; 75 | 76 | // Process covariance matrix 77 | alt_Q_t m_Q; 78 | 79 | // Array with covariances of each fused measurement 80 | std::vector m_R_multi; 81 | 82 | // parameter deciding whether to use repredictor or classic lkf 83 | bool m_use_repredictor = false; 84 | 85 | // Default dt 86 | double m_dt = ALT_DT; 87 | double m_dt_sq = m_dt * m_dt / 2; 88 | 89 | // Kalman filter - the core of the estimator 90 | std::unique_ptr mp_lkf; 91 | 92 | // Kalman filter vector for repredictor 93 | std::vector> mp_lkf_vector; 94 | 95 | // Variable for holding the current state and covariance 96 | alt_statecov_t m_sc; 97 | 98 | std::mutex mutex_lkf; 99 | 100 | bool m_is_initialized = false; 101 | 102 | bool m_got_height_measurement = false; 103 | }; 104 | 105 | } // namespace mrs_uav_odometry 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /include/AltitudeEstimatorAloamGarm.h: -------------------------------------------------------------------------------- 1 | #ifndef ALTITUDE_ESTIMATOR_ALOAMGARM_H 2 | #define ALTITUDE_ESTIMATOR_ALOAMGARM_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "AltitudeEstimator.h" 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include "types.h" 26 | 27 | #include 28 | 29 | #define ALT_DT 0.01 30 | #define ALT_INPUT_COEFF 0.10 31 | 32 | namespace mrs_uav_odometry 33 | { 34 | 35 | 36 | class AltitudeEstimatorAloamGarm : public AltitudeEstimator { 37 | 38 | public: 39 | AltitudeEstimatorAloamGarm(const std::string &estimator_name, const std::vector &fusing_measurement, const std::vector &H_multi, const var_alt_Q_t &Q, 40 | const std::vector &R_multi, const ros::NodeHandle &nh, const bool use_repredictor = false); 41 | 42 | bool doPrediction(const double input, const double dt, const ros::Time &input_stamp = ros::Time::now(), const ros::Time &predict_stamp = ros::Time::now()); 43 | bool doPrediction(const double input, const ros::Time &input_stamp = ros::Time::now(), const ros::Time &predict_stamp = ros::Time::now()); 44 | bool doCorrection(const double &measurement, int measurement_type, const ros::Time &meas_stamp, 45 | const ros::Time &predict_stamp, const std::string &measurement_name); 46 | 47 | bool getStates(algarm_alt_x_t &x); 48 | bool getStates(alt_x_t &x); 49 | bool getState(int state_id, double &state_val); 50 | std::string getName(void); 51 | bool setState(int state_id, const double &state_val); 52 | bool setR(double R, int measurement_type); 53 | bool getR(double &R, int measurement_type); 54 | bool setQ(double cov, const Eigen::Vector2i &idx); 55 | bool getQ(double &cov, const Eigen::Vector2i &idx); 56 | bool setInputCoeff(double coeff); 57 | bool getCovariance(alt_P_t &P); 58 | bool setCovariance(const alt_P_t &P); 59 | bool reset(const alt_x_t &states); 60 | 61 | void callbackSlamEigenvalues(const mrs_msgs::Float64ArrayStampedConstPtr &msg); 62 | 63 | private: 64 | std::string m_estimator_name; 65 | std::vector m_fusing_measurement; 66 | int m_n_states; 67 | size_t m_n_measurement_types; 68 | 69 | // repredictor 70 | std::unique_ptr mp_rep; 71 | 72 | // State transition matrix 73 | algarm_alt_A_t m_A; 74 | 75 | // Input matrix 76 | algarm_alt_B_t m_B; 77 | 78 | // Input coefficient 79 | double m_b = ALT_INPUT_COEFF; 80 | 81 | // Array with mapping matrices for each fused measurement 82 | std::vector m_H_multi; 83 | std::vector m_H_multi_orig; 84 | 85 | // Process covariance matrix 86 | algarm_alt_Q_t m_Q; 87 | var_alt_Q_t m_Q_orig; 88 | 89 | // Array with covariances of each fused measurement 90 | std::vector m_R_multi; 91 | 92 | ros::NodeHandle m_nh; 93 | 94 | // parameter deciding whether to use repredictor or classic lkf 95 | bool m_use_repredictor = false; 96 | 97 | // subscriber for SLAM eigenvalues 98 | ros::Subscriber m_eigenvalue_subscriber; 99 | 100 | // debug publishers 101 | ros::Publisher debug_state_publisher; 102 | ros::Publisher debug_cov_publisher; 103 | ros::Publisher debug_Q_publisher; 104 | ros::Publisher debug_duration_publisher; 105 | ros::Publisher debug_aloam_ok_publisher; 106 | ros::Publisher debug_median_publisher; 107 | 108 | // Default dt 109 | double m_dt = ALT_DT; 110 | double m_dt_sq = m_dt * m_dt / 2; 111 | 112 | // Kalman filter - the core of the estimator 113 | std::unique_ptr mp_lkf; 114 | 115 | // Kalman filter vector for repredictor 116 | std::vector> mp_lkf_vector; 117 | 118 | // Variable for holding the current state and covariance 119 | algarm_alt_statecov_t m_sc; 120 | 121 | std::mutex mutex_lkf; 122 | 123 | int m_garmin_biased_id; 124 | int m_aloam_biased_id; 125 | int m_baro_biased_id; 126 | 127 | bool m_aloam_ok = false; 128 | float m_aloam_eig = 0; 129 | bool m_eigenvalue_received = false; 130 | 131 | mrs_lib::MedianFilter m_median_filter; 132 | 133 | bool m_is_initialized = false; 134 | 135 | bool m_altitude_correction_received = false; 136 | 137 | // Config params 138 | float _mf_changes_buffer_size_; 139 | float _mf_changes_max_diff_; 140 | float _mf_changes_max_diff_close_to_ground_; 141 | float _mf_close_to_ground_threshold_; 142 | float _q_factor_range_bias_slam_ok_; 143 | float _q_factor_range_bias_range_jump_; 144 | float _q_factor_slam_bias_; 145 | float _r_factor_range_jump_; 146 | float _r_factor_slam_bad_; 147 | algarm_alt_H_t _H_range_biased_; 148 | algarm_alt_H_t _H_slam_biased_; 149 | algarm_alt_H_t _H_baro_biased_; 150 | float _q_biases_; 151 | int _biased_state_count_; 152 | float _eigenvalue_hysteresis_upper_; 153 | float _eigenvalue_hysteresis_lower_; 154 | int _repredictor_buffer_size_; 155 | bool _debug_; 156 | int _nis_buffer_size_; 157 | double _nis_threshold_; 158 | double _nis_avg_threshold_; 159 | Eigen::Matrix _initial_state_; 160 | Eigen::Matrix _initial_cov_; 161 | double _initial_time_; 162 | bool _use_initial_conditions_; 163 | ros::Time _initial_time_stamp_; 164 | 165 | bool m_close_to_ground = false; 166 | 167 | bool m_got_height_measurement = false; 168 | 169 | }; 170 | 171 | } // namespace mrs_uav_odometry 172 | 173 | #endif 174 | -------------------------------------------------------------------------------- /include/HeadingEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef HEADING_ESTIMATOR_H 2 | #define HEADING_ESTIMATOR_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "types.h" 19 | 20 | #define HDG_DT 0.01 21 | #define HDG_INPUT_COEFF 0.2 22 | 23 | namespace mrs_uav_odometry 24 | { 25 | 26 | class HeadingEstimator { 27 | 28 | public: 29 | HeadingEstimator(const std::string &estimator_name, const std::vector &fusing_measurement, const std::vector &H_multi, const hdg_Q_t &Q, 30 | const std::vector &R_multi, const bool use_repredictor = false); 31 | 32 | bool doPrediction(const hdg_u_t &input, double dt, const ros::Time &input_stamp = ros::Time::now(), const ros::Time &predict_stamp = ros::Time::now()); 33 | bool doPrediction(const hdg_u_t &input, const ros::Time &input_stamp = ros::Time::now(), const ros::Time &predict_stamp = ros::Time::now()); 34 | bool doCorrection(const double measurement, int measurement_type, const ros::Time &meas_stamp = ros::Time::now(), 35 | const ros::Time &predict_stamp = ros::Time::now()); 36 | bool getStates(hdg_x_t &states); 37 | bool getState(int state_id, double &state); 38 | std::string getName(void); 39 | bool setState(int state_id, const double state); 40 | bool setR(double cov, int measurement_type); 41 | bool getR(double &cov, int measurement_type); 42 | bool getCovariance(hdg_P_t &cov); 43 | bool setCovariance(const hdg_P_t &cov); 44 | bool reset(const hdg_x_t &states); 45 | 46 | private: 47 | std::string m_estimator_name; 48 | std::vector m_fusing_measurement; 49 | int m_n_states; 50 | size_t m_n_measurement_types; 51 | 52 | // repredictor buffer size 53 | const unsigned m_buf_sz = 200; 54 | 55 | // repredictor 56 | std::unique_ptr mp_rep; 57 | 58 | // State transition matrix 59 | hdg_A_t m_A; 60 | 61 | // Input matrix 62 | hdg_B_t m_B; 63 | 64 | // Input coefficient 65 | double m_b = HDG_INPUT_COEFF; 66 | 67 | // Array with mapping matrices for each fused measurement 68 | std::vector m_H_multi; 69 | 70 | // Process covariance matrix 71 | hdg_Q_t m_Q; 72 | 73 | // Array with covariances of each fused measurement 74 | std::vector m_R_multi; 75 | 76 | // parameter deciding whether to use repredictor or classic lkf 77 | bool m_use_repredictor = false; 78 | 79 | // Default dt 80 | double m_dt = HDG_DT; 81 | double m_dt_sq = m_dt * m_dt / 2; 82 | 83 | // Kalman filter - the core of the estimator 84 | std::unique_ptr mp_lkf; 85 | 86 | // Kalman filter vector for repredictor 87 | std::vector> mp_lkf_vector; 88 | 89 | // Variable for holding the current state and covariance 90 | hdg_statecov_t m_sc; 91 | 92 | std::mutex mutex_lkf; 93 | 94 | bool m_is_initialized = false; 95 | }; 96 | 97 | } // namespace mrs_uav_odometry 98 | 99 | #endif 100 | -------------------------------------------------------------------------------- /include/StateEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef STATE_ESTIMATOR_H 2 | #define STATE_ESTIMATOR_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | namespace mrs_uav_odometry 21 | { 22 | 23 | using statecov_t = mrs_lib::LKF_MRS_odom::statecov_t; 24 | using x_t = mrs_lib::LKF_MRS_odom::x_t; 25 | using P_t = mrs_lib::LKF_MRS_odom::P_t; 26 | using u_t = mrs_lib::LKF_MRS_odom::u_t; 27 | using z_t = mrs_lib::LKF_MRS_odom::z_t; 28 | using R_t = mrs_lib::LKF_MRS_odom::R_t; 29 | using H_t = mrs_lib::LKF_MRS_odom::H_t; 30 | 31 | class StateEstimator { 32 | 33 | public: 34 | StateEstimator(const std::string &estimator_name, const std::vector &fusing_measurement, const LatMat &Q, const std::vector &H, 35 | const std::vector &R_arr, const bool use_repredictor = false); 36 | 37 | 38 | bool doPrediction(const Vec2 &input, const double dt, const ros::Time &input_stamp = ros::Time::now(), const ros::Time &predict_stamp = ros::Time::now()); 39 | bool doCorrection(const Vec2 &measurement, int measurement_type, const ros::Time &meas_stamp = ros::Time::now(), 40 | const ros::Time &predict_stamp = ros::Time::now()); 41 | bool getStates(LatState2D &states); 42 | bool getState(int state_id, Vec2 &state); 43 | std::string getName(void); 44 | bool setState(int state_id, const Vec2 &state); 45 | bool setStates(LatState2D &states); 46 | bool setR(double cov, int measurement_type); 47 | bool getR(double &cov, int measurement_type); 48 | bool setQ(double cov, const Eigen::Vector2i &idx); 49 | bool getQ(double &cov, const Eigen::Vector2i &idx); 50 | bool getQ(double &cov, int diag); 51 | bool setQ(double cov, int diag, const std::vector &except); 52 | bool reset(const LatState2D &states); 53 | 54 | private: 55 | std::string m_estimator_name; 56 | std::vector m_fusing_measurement; 57 | LatMat m_Q; 58 | std::vector m_H; 59 | std::vector m_R_arr; 60 | 61 | int m_n_states; 62 | size_t m_n_measurement_types; 63 | 64 | // repredictor buffer size 65 | const unsigned m_buf_sz = 200; 66 | 67 | // repredictor 68 | std::unique_ptr mp_rep_x; 69 | std::unique_ptr mp_rep_y; 70 | 71 | // parameter deciding whether to use repredictor or classic lkf 72 | bool m_use_repredictor = false; 73 | 74 | std::unique_ptr mp_lkf_x; 75 | std::unique_ptr mp_lkf_y; 76 | 77 | // Kalman filter vector for repredictor 78 | std::vector> mp_lkf_vector; 79 | 80 | mrs_lib::LKF_MRS_odom::statecov_t sc_x; 81 | mrs_lib::LKF_MRS_odom::statecov_t sc_y; 82 | 83 | 84 | std::mutex mutex_lkf; 85 | 86 | bool m_is_initialized = false; 87 | }; 88 | 89 | } // namespace mrs_uav_odometry 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /include/support.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef SUPPORT_H 3 | #define SUPPORT_H 4 | 5 | /* includes //{ */ 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | //} 16 | 17 | namespace mrs_uav_odometry 18 | { 19 | 20 | /* tf2FromPose() //{ */ 21 | 22 | tf2::Transform tf2FromPose(const geometry_msgs::Pose& pose_in) { 23 | 24 | tf2::Vector3 position(pose_in.position.x, pose_in.position.y, pose_in.position.z); 25 | 26 | tf2::Quaternion q; 27 | tf2::fromMsg(pose_in.orientation, q); 28 | 29 | tf2::Transform tf_out; 30 | tf_out.setOrigin(position); 31 | tf_out.setRotation(q); 32 | tf_out.inverse(); 33 | 34 | return tf_out; 35 | } 36 | 37 | //} 38 | 39 | /* poseFromTf2() //{ */ 40 | 41 | geometry_msgs::Pose poseFromTf2(const tf2::Transform& tf_in) { 42 | 43 | geometry_msgs::Pose pose_out; 44 | pose_out.position.x = tf_in.getOrigin().getX(); 45 | pose_out.position.y = tf_in.getOrigin().getY(); 46 | pose_out.position.z = tf_in.getOrigin().getZ(); 47 | 48 | pose_out.orientation = tf2::toMsg(tf_in.getRotation()); 49 | 50 | return pose_out; 51 | } 52 | 53 | //} 54 | 55 | /* pointToVector3() //{ */ 56 | 57 | geometry_msgs::Vector3 pointToVector3(const geometry_msgs::Point& point_in) { 58 | 59 | geometry_msgs::Vector3 vec_out; 60 | vec_out.x = point_in.x; 61 | vec_out.y = point_in.y; 62 | vec_out.z = point_in.z; 63 | 64 | return vec_out; 65 | } 66 | 67 | //} 68 | 69 | /* distance() //{ */ 70 | double distance(const nav_msgs::Odometry& odom1, const nav_msgs::Odometry& odom2) { 71 | return std::sqrt(pow(odom1.pose.pose.position.x - odom2.pose.pose.position.x, 2) + pow(odom1.pose.pose.position.y - odom2.pose.pose.position.y, 2) + 72 | pow(odom1.pose.pose.position.z - odom2.pose.pose.position.z, 2)); 73 | } 74 | //} 75 | 76 | /* toLowercase //{ */ 77 | 78 | std::string toLowercase(const std::string str_in) { 79 | std::string str_out = str_in; 80 | std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::tolower); 81 | return str_out; 82 | } 83 | 84 | //} 85 | 86 | /* toUppercase //{ */ 87 | 88 | std::string toUppercase(const std::string str_in) { 89 | std::string str_out = str_in; 90 | std::transform(str_out.begin(), str_out.end(), str_out.begin(), ::toupper); 91 | return str_out; 92 | } 93 | 94 | //} 95 | 96 | /* tryPublish() //{ */ 97 | 98 | template 99 | void tryPublish(const ros::Publisher& pub, const MsgType& msg) { 100 | 101 | try { 102 | pub.publish(msg); 103 | } 104 | catch (...) { 105 | ROS_ERROR("[Odometry]: Exception caught during publishing topic %s.", pub.getTopic().c_str()); 106 | } 107 | } 108 | 109 | //} 110 | 111 | /* noNans() //{ */ 112 | bool noNans(const geometry_msgs::TransformStamped& tf) { 113 | 114 | return (std::isfinite(tf.transform.rotation.z) && std::isfinite(tf.transform.translation.x) && std::isfinite(tf.transform.translation.z)); 115 | } 116 | //} 117 | 118 | /* noNans() //{ */ 119 | bool noNans(const geometry_msgs::Quaternion& q) { 120 | 121 | return (std::isfinite(q.x) && std::isfinite(q.y) && std::isfinite(q.z) && std::isfinite(q.w)); 122 | } 123 | //} 124 | 125 | /* isZeroQuaternion() //{ */ 126 | bool isZeroQuaternion(const geometry_msgs::Quaternion& q) { 127 | 128 | return (q.x == 0 && q.y == 0 && q.z == 0 && q.w == 0); 129 | } 130 | //} 131 | 132 | /* publishTFFromOdom() //{*/ 133 | void publishTFFromOdom(const nav_msgs::Odometry& odom, const std::shared_ptr& broadcaster, const ros::Time& time) { 134 | 135 | tf2::Transform tf, tf_inv; 136 | tf = tf2FromPose(odom.pose.pose); 137 | tf_inv = tf.inverse(); 138 | 139 | geometry_msgs::Pose pose_inv; 140 | pose_inv = mrs_uav_odometry::poseFromTf2(tf_inv); 141 | 142 | geometry_msgs::TransformStamped tf_msg; 143 | tf_msg.transform.translation = pointToVector3(pose_inv.position); 144 | tf_msg.transform.rotation = pose_inv.orientation; 145 | 146 | if (noNans(tf_msg)) { 147 | 148 | tf_msg.header.stamp = time; 149 | tf_msg.header.frame_id = odom.child_frame_id; 150 | tf_msg.child_frame_id = odom.header.frame_id; 151 | 152 | try { 153 | broadcaster->sendTransform(tf_msg); 154 | } 155 | catch (...) { 156 | ROS_ERROR("[Odometry]: Exception caught during publishing TF: %s - %s.", tf_msg.child_frame_id.c_str(), tf_msg.header.frame_id.c_str()); 157 | } 158 | } else { 159 | ROS_WARN_THROTTLE(1.0, "[Odometry]: NaN detected in transform from %s to %s. Not publishing tf.", tf_msg.header.frame_id.c_str(), 160 | tf_msg.child_frame_id.c_str()); 161 | } 162 | } 163 | 164 | void publishTFFromOdom(const nav_msgs::Odometry& odom, const std::shared_ptr& broadcaster) { 165 | publishTFFromOdom(odom, broadcaster, ros::Time::now()); 166 | } 167 | 168 | /*//}*/ 169 | 170 | } // namespace mrs_uav_odometry 171 | 172 | #endif 173 | -------------------------------------------------------------------------------- /include/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef MRS_UAV_ODOMETRY_TYPES_H 3 | #define MRS_UAV_ODOMETRY_TYPES_H 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | /* defines //{ */ 13 | 14 | // trackers 15 | 16 | #define NULL_TRACKER "NullTracker" 17 | #define LANDOFF_TRACKER "LandoffTracker" 18 | 19 | // lateral 20 | #define LAT_N_STATES 3 21 | #define LAT_M_INPUTS 1 22 | #define LAT_P_MEASUREMENTS 1 23 | 24 | // altitude 25 | #define ALT_N_STATES 3 26 | #define ALT_M_INPUTS 1 27 | #define ALT_P_MEASUREMENTS 1 28 | 29 | // heading 30 | #define HDG_N_STATES 3 31 | #define HDG_M_INPUTS 2 32 | #define HDG_P_MEASUREMENTS 1 33 | 34 | /*//}*/ 35 | 36 | /* typedefs //{ */ 37 | 38 | typedef Eigen::Matrix Mat1; 39 | typedef Eigen::Matrix Vec2; 40 | 41 | typedef Eigen::Matrix LatMat; 42 | typedef Eigen::Matrix LatState1D; 43 | typedef Eigen::Matrix LatStateCol1D; 44 | typedef Eigen::Matrix LatState2D; 45 | 46 | // lateral 47 | typedef mrs_lib::LKF lkf_lat_t; 48 | typedef lkf_lat_t::statecov_t lat_statecov_t; 49 | typedef lkf_lat_t::x_t lat_x_t; 50 | typedef lkf_lat_t::u_t lat_u_t; 51 | typedef lkf_lat_t::z_t lat_z_t; 52 | typedef lkf_lat_t::A_t lat_A_t; 53 | typedef lkf_lat_t::B_t lat_B_t; 54 | typedef lkf_lat_t::H_t lat_H_t; 55 | typedef lkf_lat_t::R_t lat_R_t; 56 | typedef lkf_lat_t::Q_t lat_Q_t; 57 | typedef lkf_lat_t::P_t lat_P_t; 58 | 59 | // altitude 60 | typedef mrs_lib::LKF lkf_alt_t; 61 | typedef lkf_alt_t::statecov_t alt_statecov_t; 62 | typedef lkf_alt_t::x_t alt_x_t; 63 | typedef lkf_alt_t::u_t alt_u_t; 64 | typedef lkf_alt_t::z_t alt_z_t; 65 | typedef lkf_alt_t::A_t alt_A_t; 66 | typedef lkf_alt_t::B_t alt_B_t; 67 | typedef lkf_alt_t::H_t alt_H_t; 68 | typedef lkf_alt_t::R_t alt_R_t; 69 | typedef lkf_alt_t::Q_t alt_Q_t; 70 | typedef lkf_alt_t::P_t alt_P_t; 71 | 72 | // altitude - repredictor 73 | typedef mrs_lib::varstepLKF var_lkf_alt_t; 74 | typedef var_lkf_alt_t::statecov_t var_alt_statecov_t; 75 | typedef var_lkf_alt_t::x_t var_alt_x_t; 76 | typedef var_lkf_alt_t::u_t var_alt_u_t; 77 | typedef var_lkf_alt_t::z_t var_alt_z_t; 78 | typedef var_lkf_alt_t::A_t var_alt_A_t; 79 | typedef var_lkf_alt_t::B_t var_alt_B_t; 80 | typedef var_lkf_alt_t::H_t var_alt_H_t; 81 | typedef var_lkf_alt_t::R_t var_alt_R_t; 82 | typedef var_lkf_alt_t::Q_t var_alt_Q_t; 83 | typedef var_lkf_alt_t::P_t var_alt_P_t; 84 | 85 | // altitude - ALOAMGARM 86 | typedef mrs_lib::JLKF<6, 1, 1, 3> algarm_alt_t; 87 | typedef algarm_alt_t::statecov_t algarm_alt_statecov_t; 88 | typedef algarm_alt_t::x_t algarm_alt_x_t; 89 | typedef algarm_alt_t::u_t algarm_alt_u_t; 90 | typedef algarm_alt_t::z_t algarm_alt_z_t; 91 | typedef algarm_alt_t::A_t algarm_alt_A_t; 92 | typedef algarm_alt_t::B_t algarm_alt_B_t; 93 | typedef algarm_alt_t::H_t algarm_alt_H_t; 94 | typedef algarm_alt_t::R_t algarm_alt_R_t; 95 | typedef algarm_alt_t::Q_t algarm_alt_Q_t; 96 | typedef algarm_alt_t::P_t algarm_alt_P_t; 97 | 98 | // heading 99 | typedef mrs_lib::LKF lkf_hdg_t; 100 | typedef lkf_hdg_t::statecov_t hdg_statecov_t; 101 | typedef lkf_hdg_t::x_t hdg_x_t; 102 | typedef lkf_hdg_t::u_t hdg_u_t; 103 | typedef lkf_hdg_t::z_t hdg_z_t; 104 | typedef lkf_hdg_t::A_t hdg_A_t; 105 | typedef lkf_hdg_t::B_t hdg_B_t; 106 | typedef lkf_hdg_t::H_t hdg_H_t; 107 | typedef lkf_hdg_t::R_t hdg_R_t; 108 | typedef lkf_hdg_t::Q_t hdg_Q_t; 109 | typedef lkf_hdg_t::P_t hdg_P_t; 110 | 111 | // heading - variable LKF for repredictor 112 | typedef mrs_lib::varstepLKF var_lkf_hdg_t; 113 | typedef lkf_hdg_t::statecov_t var_hdg_statecov_t; 114 | typedef lkf_hdg_t::x_t var_hdg_x_t; 115 | typedef lkf_hdg_t::u_t var_hdg_u_t; 116 | typedef lkf_hdg_t::z_t var_hdg_z_t; 117 | typedef lkf_hdg_t::A_t var_hdg_A_t; 118 | typedef lkf_hdg_t::B_t var_hdg_B_t; 119 | typedef lkf_hdg_t::H_t var_hdg_H_t; 120 | typedef lkf_hdg_t::R_t var_hdg_R_t; 121 | typedef lkf_hdg_t::Q_t var_hdg_Q_t; 122 | typedef lkf_hdg_t::P_t var_hdg_P_t; 123 | 124 | typedef mrs_lib::Repredictor rep_t; 125 | typedef mrs_lib::Repredictor rep_lat_t; 126 | typedef mrs_lib::Repredictor rep_hdg_t; 127 | typedef mrs_lib::RepredictorAloamgarm algarm_rep_t; 128 | 129 | //} 130 | 131 | #endif 132 | -------------------------------------------------------------------------------- /launch/gazebo/gazebo_altitude.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 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /launch/gazebo/gazebo_grass_plane.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 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /launch/odometry.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 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | -------------------------------------------------------------------------------- /launch/rtk_republisher.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 | -------------------------------------------------------------------------------- /launch/uwb_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mrs_uav_odometry 5 | 1.0.4 6 | The mrs_uav_odometry package 7 | 8 | Matej Petrlik 9 | Matej Petrlik 10 | 11 | BSD 3-Clause 12 | 13 | catkin 14 | 15 | geometry_msgs 16 | sensor_msgs 17 | nav_msgs 18 | tf2_geometry_msgs 19 | roscpp 20 | std_msgs 21 | tf 22 | cmake_modules 23 | mrs_lib 24 | mrs_msgs 25 | mavros_msgs 26 | nodelet 27 | dynamic_reconfigure 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /plot_juggler/odometry.xml: -------------------------------------------------------------------------------- 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 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | -------------------------------------------------------------------------------- /plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Odometry nodelet 4 | 5 | 6 | 7 | 8 | 9 | RtkRepublisher nodelet 10 | 11 | 12 | -------------------------------------------------------------------------------- /rviz/odometry.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | - /position_cmd1/Covariance1/Orientation1 9 | - /ground_truth1/Shape1 10 | - /ground_truth1/Covariance1 11 | - /odom_gps1/Shape1 12 | - /odom_stabilization1/Shape1 13 | - /TF1/Frames1 14 | Splitter Ratio: 0.5 15 | Tree Height: 1031 16 | - Class: rviz/Selection 17 | Name: Selection 18 | - Class: rviz/Tool Properties 19 | Expanded: 20 | - /2D Pose Estimate1 21 | - /2D Nav Goal1 22 | - /Publish Point1 23 | Name: Tool Properties 24 | Splitter Ratio: 0.5886790156364441 25 | - Class: rviz/Views 26 | Expanded: 27 | - /Current View1 28 | Name: Views 29 | Splitter Ratio: 0.5 30 | - Class: rviz/Time 31 | Experimental: false 32 | Name: Time 33 | SyncMode: 0 34 | SyncSource: "" 35 | Preferences: 36 | PromptSaveOnExit: true 37 | Visualization Manager: 38 | Class: "" 39 | Displays: 40 | - Alpha: 0.5 41 | Cell Size: 1 42 | Class: rviz/Grid 43 | Color: 160; 160; 164 44 | Enabled: true 45 | Line Style: 46 | Line Width: 0.029999999329447746 47 | Value: Lines 48 | Name: Grid 49 | Normal Cell Count: 0 50 | Offset: 51 | X: 0 52 | Y: 0 53 | Z: 0 54 | Plane: XY 55 | Plane Cell Count: 100 56 | Reference Frame: uav1/local_origin 57 | Value: true 58 | - Angle Tolerance: 0.009999999776482582 59 | Class: rviz/Odometry 60 | Covariance: 61 | Orientation: 62 | Alpha: 0.5 63 | Color: 255; 255; 127 64 | Color Style: Unique 65 | Frame: Local 66 | Offset: 1 67 | Scale: 1 68 | Value: true 69 | Position: 70 | Alpha: 0.30000001192092896 71 | Color: 204; 51; 204 72 | Scale: 1 73 | Value: true 74 | Value: true 75 | Enabled: true 76 | Keep: 1 77 | Name: position_cmd 78 | Position Tolerance: 0.009999999776482582 79 | Shape: 80 | Alpha: 1 81 | Axes Length: 1 82 | Axes Radius: 0.10000000149011612 83 | Color: 255; 0; 0 84 | Head Length: 0.30000001192092896 85 | Head Radius: 0.10000000149011612 86 | Shaft Length: 1 87 | Shaft Radius: 0.05000000074505806 88 | Value: Arrow 89 | Topic: /uav1/control_manager/cmd_odom 90 | Unreliable: true 91 | Value: true 92 | - Angle Tolerance: 0.009999999776482582 93 | Class: rviz/Odometry 94 | Covariance: 95 | Orientation: 96 | Alpha: 0.5 97 | Color: 255; 255; 127 98 | Color Style: Unique 99 | Frame: Local 100 | Offset: 1 101 | Scale: 1 102 | Value: true 103 | Position: 104 | Alpha: 0.30000001192092896 105 | Color: 204; 51; 204 106 | Scale: 1 107 | Value: true 108 | Value: true 109 | Enabled: true 110 | Keep: 1 111 | Name: odom_main 112 | Position Tolerance: 0.009999999776482582 113 | Shape: 114 | Alpha: 1 115 | Axes Length: 1 116 | Axes Radius: 0.10000000149011612 117 | Color: 0; 25; 255 118 | Head Length: 0.30000001192092896 119 | Head Radius: 0.10000000149011612 120 | Shaft Length: 1 121 | Shaft Radius: 0.05000000074505806 122 | Value: Arrow 123 | Topic: /uav1/odometry/odom_main 124 | Unreliable: true 125 | Value: true 126 | - Angle Tolerance: 0.009999999776482582 127 | Class: rviz/Odometry 128 | Covariance: 129 | Orientation: 130 | Alpha: 0.5 131 | Color: 255; 255; 127 132 | Color Style: Unique 133 | Frame: Local 134 | Offset: 1 135 | Scale: 1 136 | Value: true 137 | Position: 138 | Alpha: 0.30000001192092896 139 | Color: 204; 51; 204 140 | Scale: 1 141 | Value: true 142 | Value: true 143 | Enabled: true 144 | Keep: 1 145 | Name: ground_truth 146 | Position Tolerance: 0.009999999776482582 147 | Shape: 148 | Alpha: 1 149 | Axes Length: 1 150 | Axes Radius: 0.10000000149011612 151 | Color: 20; 200; 0 152 | Head Length: 0.30000001192092896 153 | Head Radius: 0.10000000149011612 154 | Shaft Length: 1 155 | Shaft Radius: 0.05000000074505806 156 | Value: Arrow 157 | Topic: /uav1/odometry/rtk_local_odom 158 | Unreliable: true 159 | Value: true 160 | - Alpha: 1 161 | Arrow Length: 0.30000001192092896 162 | Axes Length: 0.30000001192092896 163 | Axes Radius: 0.009999999776482582 164 | Class: rviz/PoseArray 165 | Color: 0; 0; 255 166 | Enabled: true 167 | Head Length: 0.07000000029802322 168 | Head Radius: 0.029999999329447746 169 | Name: predicted_trajectory 170 | Shaft Length: 0.23000000417232513 171 | Shaft Radius: 0.009999999776482582 172 | Shape: Arrow (Flat) 173 | Topic: /uav1/control_manager/mpc_tracker/predicted_trajectory_debugging 174 | Unreliable: true 175 | Value: true 176 | - Angle Tolerance: 0.009999999776482582 177 | Class: rviz/Odometry 178 | Covariance: 179 | Orientation: 180 | Alpha: 0.5 181 | Color: 255; 255; 127 182 | Color Style: Unique 183 | Frame: Local 184 | Offset: 1 185 | Scale: 1 186 | Value: true 187 | Position: 188 | Alpha: 0.30000001192092896 189 | Color: 204; 51; 204 190 | Scale: 1 191 | Value: true 192 | Value: true 193 | Enabled: true 194 | Keep: 1 195 | Name: odom_gps 196 | Position Tolerance: 0.009999999776482582 197 | Shape: 198 | Alpha: 1 199 | Axes Length: 1 200 | Axes Radius: 0.10000000149011612 201 | Color: 255; 255; 0 202 | Head Length: 0.30000001192092896 203 | Head Radius: 0.10000000149011612 204 | Shaft Length: 1 205 | Shaft Radius: 0.05000000074505806 206 | Value: Arrow 207 | Topic: /uav1/odometry/odom_gps 208 | Unreliable: true 209 | Value: true 210 | - Angle Tolerance: 0.009999999776482582 211 | Class: rviz/Odometry 212 | Covariance: 213 | Orientation: 214 | Alpha: 0.5 215 | Color: 255; 255; 127 216 | Color Style: Unique 217 | Frame: Local 218 | Offset: 1 219 | Scale: 1 220 | Value: true 221 | Position: 222 | Alpha: 0.30000001192092896 223 | Color: 204; 51; 204 224 | Scale: 1 225 | Value: true 226 | Value: true 227 | Enabled: true 228 | Keep: 1 229 | Name: odom_optflow 230 | Position Tolerance: 0.009999999776482582 231 | Shape: 232 | Alpha: 1 233 | Axes Length: 1 234 | Axes Radius: 0.10000000149011612 235 | Color: 255; 85; 255 236 | Head Length: 0.30000001192092896 237 | Head Radius: 0.10000000149011612 238 | Shaft Length: 1 239 | Shaft Radius: 0.05000000074505806 240 | Value: Arrow 241 | Topic: /uav1/odometry/odom_optflow 242 | Unreliable: true 243 | Value: true 244 | - Alpha: 1 245 | Arrow Length: 0.30000001192092896 246 | Axes Length: 0.30000001192092896 247 | Axes Radius: 0.009999999776482582 248 | Class: rviz/PoseArray 249 | Color: 255; 25; 0 250 | Enabled: true 251 | Head Length: 0.07000000029802322 252 | Head Radius: 0.029999999329447746 253 | Name: trajectory 254 | Shaft Length: 0.23000000417232513 255 | Shaft Radius: 0.009999999776482582 256 | Shape: Arrow (Flat) 257 | Topic: /uav1/control_manager/mpc_tracker/debug_set_trajectory 258 | Unreliable: false 259 | Value: true 260 | - Alpha: 0.10000000149011612 261 | Class: mrs_rviz_plugins/MRS_Bumper 262 | Collision alpha: 0.5 263 | Collision color: 255; 0; 0 264 | Color: 204; 51; 204 265 | Colorize collisions: true 266 | Display mode: sensor types 267 | Enabled: true 268 | History Length: 1 269 | Horizontal collision threshold: 1 270 | Name: MRS_Bumper 271 | Show sectors with no data: false 272 | Show undetected obstacles: false 273 | Topic: /uav1/bumper/obstacle_sectors 274 | Unreliable: false 275 | Value: true 276 | Vertical collision threshold: 1 277 | - Alpha: 1 278 | Autocompute Intensity Bounds: true 279 | Autocompute Value Bounds: 280 | Max Value: 10 281 | Min Value: -10 282 | Value: true 283 | Axis: Z 284 | Channel Name: intensity 285 | Class: rviz/LaserScan 286 | Color: 255; 255; 255 287 | Color Transformer: Intensity 288 | Decay Time: 0 289 | Enabled: true 290 | Invert Rainbow: false 291 | Max Color: 255; 255; 255 292 | Max Intensity: 0 293 | Min Color: 0; 0; 0 294 | Min Intensity: 0 295 | Name: lidar_scan 296 | Position Transformer: XYZ 297 | Queue Size: 10 298 | Selectable: true 299 | Size (Pixels): 3 300 | Size (m): 0.05000000074505806 301 | Style: Flat Squares 302 | Topic: /uav1/rplidar/scan 303 | Unreliable: false 304 | Use Fixed Frame: true 305 | Use rainbow: true 306 | Value: true 307 | - Angle Tolerance: 0.009999999776482582 308 | Class: rviz/Odometry 309 | Covariance: 310 | Orientation: 311 | Alpha: 0.5 312 | Color: 255; 255; 127 313 | Color Style: Unique 314 | Frame: Local 315 | Offset: 1 316 | Scale: 1 317 | Value: true 318 | Position: 319 | Alpha: 0.30000001192092896 320 | Color: 204; 51; 204 321 | Scale: 1 322 | Value: true 323 | Value: true 324 | Enabled: true 325 | Keep: 1 326 | Name: odom_stabilization 327 | Position Tolerance: 0.009999999776482582 328 | Shape: 329 | Alpha: 1 330 | Axes Length: 1 331 | Axes Radius: 0.10000000149011612 332 | Color: 255; 170; 0 333 | Head Length: 0.30000001192092896 334 | Head Radius: 0.10000000149011612 335 | Shaft Length: 1 336 | Shaft Radius: 0.05000000074505806 337 | Value: Arrow 338 | Topic: /uav1/lidar_stabilization/odom 339 | Unreliable: false 340 | Value: true 341 | - Alpha: 0.5 342 | Autocompute Intensity Bounds: true 343 | Autocompute Value Bounds: 344 | Max Value: 10 345 | Min Value: -10 346 | Value: true 347 | Axis: Z 348 | Channel Name: intensity 349 | Class: rviz/PointCloud2 350 | Color: 0; 0; 255 351 | Color Transformer: FlatColor 352 | Decay Time: 0 353 | Enabled: true 354 | Invert Rainbow: false 355 | Max Color: 255; 255; 255 356 | Max Intensity: 4096 357 | Min Color: 0; 0; 0 358 | Min Intensity: 0 359 | Name: aligned_scan 360 | Position Transformer: XYZ 361 | Queue Size: 10 362 | Selectable: true 363 | Size (Pixels): 3 364 | Size (m): 0.10000000149011612 365 | Style: Flat Squares 366 | Topic: /uav1/lidar_stabilization/pc_seq_transformed 367 | Unreliable: false 368 | Use Fixed Frame: true 369 | Use rainbow: true 370 | Value: true 371 | - Alpha: 0.5 372 | Autocompute Intensity Bounds: true 373 | Autocompute Value Bounds: 374 | Max Value: 10 375 | Min Value: -10 376 | Value: true 377 | Axis: Z 378 | Channel Name: intensity 379 | Class: rviz/PointCloud2 380 | Color: 0; 255; 0 381 | Color Transformer: FlatColor 382 | Decay Time: 0 383 | Enabled: true 384 | Invert Rainbow: false 385 | Max Color: 255; 255; 255 386 | Max Intensity: 4096 387 | Min Color: 0; 0; 0 388 | Min Intensity: 0 389 | Name: previous_scan 390 | Position Transformer: XYZ 391 | Queue Size: 10 392 | Selectable: true 393 | Size (Pixels): 3 394 | Size (m): 0.10000000149011612 395 | Style: Flat Squares 396 | Topic: /uav1/lidar_stabilization/pc_seq_reference 397 | Unreliable: false 398 | Use Fixed Frame: true 399 | Use rainbow: true 400 | Value: true 401 | - Class: rviz/TF 402 | Enabled: true 403 | Frame Timeout: 15 404 | Frames: 405 | All Enabled: false 406 | uav1/bluefox_optflow: 407 | Value: false 408 | uav1/bluefox_optflow_optical: 409 | Value: false 410 | uav1/fcu: 411 | Value: true 412 | uav1/fcu_horizontal: 413 | Value: false 414 | uav1/garmin: 415 | Value: false 416 | uav1/gps_origin: 417 | Value: true 418 | uav1/local_origin: 419 | Value: true 420 | Marker Scale: 1 421 | Name: TF 422 | Show Arrows: true 423 | Show Axes: true 424 | Show Names: true 425 | Tree: 426 | uav1/fcu: 427 | uav1/bluefox_optflow: 428 | uav1/bluefox_optflow_optical: 429 | {} 430 | uav1/fcu_horizontal: 431 | {} 432 | uav1/garmin: 433 | {} 434 | uav1/gps_origin: 435 | {} 436 | uav1/local_origin: 437 | {} 438 | Update Interval: 0 439 | Value: true 440 | Enabled: true 441 | Global Options: 442 | Background Color: 0; 0; 0 443 | Default Light: true 444 | Fixed Frame: uav1/local_origin 445 | Frame Rate: 60 446 | Name: root 447 | Tools: 448 | - Class: rviz/Interact 449 | Hide Inactive Objects: true 450 | - Class: rviz/MoveCamera 451 | - Class: rviz/Select 452 | - Class: rviz/FocusCamera 453 | - Class: rviz/Measure 454 | - Class: rviz/SetInitialPose 455 | Topic: /initialpose 456 | - Class: rviz/SetGoal 457 | Topic: /move_base_simple/goal 458 | - Class: rviz/PublishPoint 459 | Single click: true 460 | Topic: /clicked_point 461 | Value: true 462 | Views: 463 | Current: 464 | Class: rviz/ThirdPersonFollower 465 | Distance: 6.524624824523926 466 | Enable Stereo Rendering: 467 | Stereo Eye Separation: 0.05999999865889549 468 | Stereo Focal Distance: 1 469 | Swap Stereo Eyes: false 470 | Value: false 471 | Focal Point: 472 | X: 0.729694128036499 473 | Y: 0.588066816329956 474 | Z: -1.0097784996032715 475 | Focal Shape Fixed Size: true 476 | Focal Shape Size: 0.05000000074505806 477 | Invert Z Axis: false 478 | Name: Current View 479 | Near Clip Distance: 0.009999999776482582 480 | Pitch: 0.544796347618103 481 | Target Frame: fcu_uav1 482 | Value: ThirdPersonFollower (rviz) 483 | Yaw: 2.8650577068328857 484 | Saved: ~ 485 | Window Geometry: 486 | Displays: 487 | collapsed: true 488 | Height: 1176 489 | Hide Left Dock: true 490 | Hide Right Dock: true 491 | QMainWindow State: 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 492 | Selection: 493 | collapsed: false 494 | Time: 495 | collapsed: false 496 | Tool Properties: 497 | collapsed: false 498 | Views: 499 | collapsed: true 500 | Width: 1920 501 | X: 0 502 | Y: 24 503 | -------------------------------------------------------------------------------- /scripts/change_uav.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # get path to script 4 | SCRIPT_PATH="$( cd "$(dirname "$0")" ; pwd -P )" 5 | 6 | if [ -x "$(whereis nvim | awk '{print $2}')" ]; then 7 | VIM_BIN="$(whereis nvim | awk '{print $2}')" 8 | HEADLESS="--headless" 9 | elif [ -x "$(whereis vim | awk '{print $2}')" ]; then 10 | VIM_BIN="$(whereis vim | awk '{print $2}')" 11 | HEADLESS="" 12 | fi 13 | 14 | $VIM_BIN -u "$GIT_PATH/linux-setup/submodules/profile_manager/epigen/epigen.vimrc "$HEADLESS -Ens -c "%s/uav[0-9]\+/$1/g" -c "wqa" -- "$SCRIPT_PATH/../plot_juggler/odometry.xml" 15 | $VIM_BIN -u "$GIT_PATH/linux-setup/submodules/profile_manager/epigen/epigen.vimrc" $HEADLESS -Ens -c "%s/uav[0-9]\+/$1/g" -c "wqa" -- "$SCRIPT_PATH/../rviz/odometry.rviz" 16 | -------------------------------------------------------------------------------- /scripts/simulation.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SESSION_NAME=uav1 4 | UAV_NAME=$SESSION_NAME 5 | ESTIMATOR=$1 6 | SENSORS= 7 | LAUNCH_NODES= 8 | WORLD='grass_plane' 9 | 10 | if [ "$#" == 0 ]; then 11 | ESTIMATOR=gps 12 | fi 13 | 14 | if [ "$ESTIMATOR" == "optflow" ]; then 15 | SENSORS="$SENSORS --enable-bluefox-camera" 16 | LAUNCH_NODES='waitForOdometry; roslaunch mrs_optic_flow optic_flow.launch' 17 | # elif [ "$ESTIMATOR" == "gps" ]; then 18 | # SENSORS="$SENSORS --enable-rplidar" 19 | # LAUNCH_NODES='waitForOdometry; roslaunch hector_mapping uav.launch' 20 | # WORLD='worlds/cathedral.world' 21 | elif [ "$ESTIMATOR" == "hector" ]; then 22 | SENSORS="$SENSORS --enable-rplidar" 23 | LAUNCH_NODES='waitForOdometry; roslaunch hector_mapping uav.launch' 24 | WORLD='forest' 25 | elif [ "$ESTIMATOR" == "icp" ]; then 26 | SENSORS="$SENSORS --enable-rplidar" 27 | LAUNCH_NODES='waitForOdometry; roslaunch mrs_icp2d uav.launch' 28 | WORLD='worlds/forest.world' 29 | elif [ "$ESTIMATOR" == "lidar" ]; then 30 | SENSORS="$SENSORS --enable-velodyne" 31 | LAUNCH_NODES='waitForOdometry; roslaunch aloam_velodyne velodyne_odometry_simulation.launch' 32 | WORLD='worlds/forest.world' 33 | fi 34 | 35 | # following commands will be executed first, in each window 36 | pre_input="export UAV_NAME=$UAV_NAME; export ATHAME_ENABLED=0; export ROS_MASTER_URI=http://localhost:11311; export ROS_IP=127.0.0.1; export ODOMETRY_TYPE=$ESTIMATOR" 37 | 38 | # define commands 39 | # 'name' 'command' 40 | input=( 41 | 'Roscore' 'roscore 42 | ' 43 | 'Gazebo' "waitForRos; roscd mrs_simulation; roslaunch mrs_simulation simulation.launch gui:=true debug:=false world_name:=$WORLD --wait 44 | " 45 | 'Spawn' "waitForSimulation; rosrun mrs_simulation spawn 1 --run --delete --$UAV_TYPE --enable-rangefinder --enable-ground-truth $SENSORS --file ~/mrs_workspace/src/uav_core/ros_packages/mrs_uav_odometry/config/init_pose/init_pose.csv 46 | " 47 | 'Status' "waitForOdometry; roslaunch mrs_uav_status status.launch 48 | " 49 | 'Core' "waitForOdometry; roslaunch mrs_uav_general core.launch 50 | " 51 | 'Bumper' "waitForOdometry; roslaunch mrs_bumper bumper.launch 52 | " 53 | 'Localization' "waitForOdometry; $LAUNCH_NODES 54 | " 55 | "Takeoff" "waitForControl; rosservice call /$UAV_NAME/control_manager/motors 1; rosservice call /$UAV_NAME/mavros/cmd/arming 1; rosservice call /$UAV_NAME/mavros/set_mode 0 offboard; rosservice call /$UAV_NAME/uav_manager/takeoff; 56 | " 57 | 'ChangeEstimator' "rosservice call /$UAV_NAME/odometry/change_estimator_type_string GPS" 58 | 'ChangeHdgEstimator' "rosservice call /$UAV_NAME/odometry/change_hdg_estimator_type_string GPS" 59 | 60 | 'Constraints' "rosservice call /$UAV_NAME/constraint_manager/set_constraints darpa" 61 | 'GoTo' "rosservice call /$UAV_NAME/control_manager/goto \"goal: [0.0, 0.0, 3.0, 0.0]\"" 62 | 'GoToRelative' "rosservice call /$UAV_NAME/control_manager/goto_relative \"goal: [0.0, 0.0, 0.0, 0.0]\"" 63 | 'GoFcu' "rosservice call /$UAV_NAME/control_manager/goto_fcu \"goal: [0.0, 0.0, 0.0, 0.0]\"" 64 | 'RVIZ' "waitForOdometry; roscd mrs_uav_odometry; ./scripts/change_uav.sh $UAV_NAME; rosrun rviz rviz -d rviz/odometry.rviz 65 | " 66 | 'rviz_interface' "waitForOdometry; roslaunch mrs_rviz_interface mrs_rviz_interface.launch 67 | " 68 | 'Juggler' "waitForOdometry; roscd mrs_uav_odometry; ./scripts/change_uav.sh $UAV_NAME; i3 workspace "9"; rosrun plotjuggler PlotJuggler -l plot_juggler/odometry.xml" 69 | 'reconfigure' " waitForOdometry; rosrun rqt_reconfigure rqt_reconfigure" 70 | # 'Layout' "waitForControl; i3 workspace "9"; ~/.i3/layout_manager.sh rviz_rqt_juggler 71 | # " 72 | 'Camera_follow' "waitForOdometry; gz camera -c gzclient_camera -f uav1 73 | " 74 | ) 75 | 76 | if [ -z ${TMUX} ]; 77 | then 78 | TMUX= tmux new-session -s $SESSION_NAME -d 79 | echo "Starting new session." 80 | else 81 | SESSION_NAME="$(tmux display-message -p '#S')" 82 | fi 83 | 84 | # create arrays of names and commands 85 | for ((i=0; i < ${#input[*]}; i++)); 86 | do 87 | ((i%2==0)) && names[$i/2]="${input[$i]}" 88 | ((i%2==1)) && cmds[$i/2]="${input[$i]}" 89 | done 90 | 91 | # run tmux windows 92 | for ((i=0; i < ${#names[*]}; i++)); 93 | do 94 | tmux new-window -t $SESSION_NAME:$(($i+10)) -n "${names[$i]}" 95 | done 96 | 97 | sleep 4 98 | 99 | # send commands 100 | for ((i=0; i < ${#cmds[*]}; i++)); 101 | do 102 | tmux send-keys -t $SESSION_NAME:$(($i+10)) "${pre_input}; 103 | ${cmds[$i]}" 104 | done 105 | 106 | sleep 4 107 | 108 | tmux select-window -t $SESSION_NAME:0 109 | tmux -2 attach-session -t $SESSION_NAME 110 | 111 | clear 112 | -------------------------------------------------------------------------------- /src/HeadingEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "HeadingEstimator.h" 2 | 3 | namespace mrs_uav_odometry 4 | { 5 | 6 | /* //{ HeadingEstimator() */ 7 | 8 | // clang-format off 9 | HeadingEstimator::HeadingEstimator( 10 | const std::string &estimator_name, 11 | const std::vector &fusing_measurement, 12 | const std::vector &H_multi, 13 | const hdg_Q_t &Q, 14 | const std::vector &R_multi, 15 | const bool use_repredictor) 16 | : 17 | m_estimator_name(estimator_name), 18 | m_fusing_measurement(fusing_measurement), 19 | m_H_multi(H_multi), 20 | m_Q(Q), 21 | m_R_multi(R_multi), 22 | m_use_repredictor(use_repredictor) 23 | { 24 | 25 | // clang-format on 26 | 27 | // Number of states 28 | m_n_states = m_A.rows(); 29 | 30 | // Number of measurement types 31 | m_n_measurement_types = m_fusing_measurement.size(); 32 | 33 | /* //{ sanity checks */ 34 | 35 | // Check size of m_R 36 | if (m_Q.rows() != m_n_states) { 37 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".HeadingEstimator()" 38 | << "): wrong size of \"R.rows()\". Should be: " << m_n_states << " is:" << m_Q.rows() << std::endl; 39 | return; 40 | } 41 | 42 | if (m_Q.cols() != m_n_states) { 43 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".HeadingEstimator()" 44 | << "): wrong size of \"R.cols()\". Should be: " << m_n_states << " is:" << m_Q.cols() << std::endl; 45 | return; 46 | } 47 | 48 | // Check size of m_H_multi 49 | if (m_H_multi.size() != m_n_measurement_types) { 50 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".HeadingEstimator()" 51 | << "): wrong size of \"m_H_multi\". Should be: " << m_n_measurement_types << " is:" << m_H_multi.size() << std::endl; 52 | return; 53 | } 54 | 55 | // Check size of m_H_multi elements 56 | for (size_t i = 0; i < m_H_multi.size(); i++) { 57 | if (m_H_multi[i].rows() != 1 || m_H_multi[i].cols() != m_n_states) { 58 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".HeadingEstimator()" 59 | << "): wrong size of \"m_H_multi[" << i << "]\". Should be: (1, " << m_n_states << ") is: (" << m_H_multi[i].rows() << ", " 60 | << m_H_multi[i].cols() << ")" << std::endl; 61 | return; 62 | } 63 | } 64 | 65 | // Check size of m_R_multi 66 | if (m_R_multi.size() != m_n_measurement_types) { 67 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".HeadingEstimator()" 68 | << "): wrong size of \"m_R_multi\". Should be: " << m_n_measurement_types << " is:" << m_R_multi.size() << std::endl; 69 | return; 70 | } 71 | 72 | // Check size of m_R_multi elements 73 | for (size_t i = 0; i < m_R_multi.size(); i++) { 74 | if (m_R_multi[i].rows() != 1 || m_R_multi[i].cols() != 1) { 75 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".HeadingEstimator()" 76 | << "): wrong size of \"m_R_multi[" << i << "]\". Should be: (1, 1) is: (" << m_R_multi[i].rows() << ", " << m_R_multi[i].cols() << ")" 77 | << std::endl; 78 | return; 79 | } 80 | } 81 | 82 | //} 83 | 84 | // clang-format off 85 | m_A << 86 | 1.0, m_dt, m_dt_sq, 87 | 0, 1-m_b, m_dt, 88 | 0, 0, 1.0; 89 | 90 | m_B << 0, 0, // TODO try with heading input 91 | 0, m_b, 92 | 0, 0; 93 | // clang-format on 94 | 95 | // set measurement mapping matrix H to zero, it will be set later during each correction step 96 | hdg_H_t m_H_zero = m_H_zero.Zero(); 97 | 98 | // Initialize all states to 0 99 | const hdg_x_t x0 = hdg_x_t::Zero(); 100 | hdg_P_t P_tmp = hdg_P_t::Identity(); 101 | const hdg_P_t P0 = 1000.0 * P_tmp * P_tmp.transpose(); 102 | const hdg_statecov_t sc0({x0, P0}); 103 | m_sc = sc0; 104 | const hdg_u_t u0 = hdg_u_t::Zero(); 105 | const ros::Time t0 = ros::Time(0); 106 | 107 | if (m_use_repredictor) { 108 | std::cout << "[HeadingEstimator]: Using repredictor in " << m_estimator_name.c_str() << " estimator." << std::endl; 109 | // Lambda functions generating A and B matrices based on dt 110 | auto generateA = [](const double dt) { 111 | var_hdg_A_t A; 112 | A << 1, dt, dt * dt / 2, 0, 1 - HDG_INPUT_COEFF, dt, 0, 0, 1; 113 | return A; 114 | }; 115 | auto generateB = []([[maybe_unused]] const double dt) { 116 | var_hdg_B_t B; 117 | B << 0, 0, 0, HDG_INPUT_COEFF, 0, 0; 118 | return B; 119 | }; 120 | // Initialize separate LKF models for each H matrix 121 | for (size_t i = 0; i < m_H_multi.size(); i++) { 122 | mp_lkf_vector.push_back(std::make_shared(generateA, generateB, m_H_multi[i])); 123 | } 124 | // Initialize repredictor 125 | mp_rep = std::make_unique(x0, P0, u0, Q, t0, mp_lkf_vector.at(0), m_buf_sz); 126 | } else { 127 | // Initialize a single LKF 128 | mp_lkf = std::make_unique(m_A, m_B, m_H_zero); 129 | } 130 | 131 | std::cout << "[HeadingEstimator]: New HeadingEstimator initialized " << std::endl; 132 | std::cout << "name: " << m_estimator_name << std::endl; 133 | 134 | std::cout << " fusing measurements: " << std::endl; 135 | for (size_t i = 0; i < m_fusing_measurement.size(); i++) { 136 | std::cout << m_fusing_measurement[i] << " "; 137 | } 138 | 139 | std::cout << std::endl << " H_multi: " << std::endl; 140 | for (size_t i = 0; i < m_H_multi.size(); i++) { 141 | std::cout << m_H_multi[i] << std::endl; 142 | } 143 | std::cout << std::endl << " R_multi: " << std::endl; 144 | for (size_t i = 0; i < m_R_multi.size(); i++) { 145 | std::cout << m_R_multi[i] << std::endl; 146 | } 147 | std::cout << std::endl << " A: " << std::endl << m_A << std::endl << " B: " << std::endl << m_B << std::endl << " Q: " << std::endl << m_Q << std::endl; 148 | 149 | m_is_initialized = true; 150 | } 151 | 152 | //} 153 | 154 | /* //{ doPrediction() */ 155 | 156 | bool HeadingEstimator::doPrediction(const hdg_u_t &input, double dt, const ros::Time &input_stamp, const ros::Time &predict_stamp) { 157 | 158 | /* //{ sanity checks */ 159 | 160 | if (!m_is_initialized) 161 | return false; 162 | 163 | 164 | // Check for NaNs 165 | for (int i = 0; i < input.size(); i++) { 166 | if (!std::isfinite(input(i))) { 167 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input(i) << ", double dt=" << dt 168 | << "): NaN detected in variable \"input(0)\"." << std::endl; 169 | return false; 170 | } 171 | } 172 | 173 | if (!std::isfinite(dt)) { 174 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input << ", double dt=" << dt 175 | << "): NaN detected in variable \"dt\"." << std::endl; 176 | return false; 177 | } 178 | 179 | // Check for non-positive dt 180 | if (dt <= 0) { 181 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input << ", double dt=" << dt 182 | << "): \"dt\" should be > 0." << std::endl; 183 | return false; 184 | } 185 | 186 | //} 187 | 188 | 189 | hdg_A_t A = m_A; 190 | /* hdg_B_t B = m_B; */ 191 | 192 | double dt_sq = std::pow(dt, 2) / 2; 193 | 194 | /* B(0, 0) = dt; */ 195 | /* B(1, 1) = dt; */ 196 | 197 | A(0, 1) = dt; 198 | A(1, 2) = dt; 199 | 200 | A(0, 2) = dt_sq; 201 | 202 | { 203 | std::scoped_lock lock(mutex_lkf); 204 | 205 | try { 206 | // Apply the prediction step 207 | if (m_use_repredictor) { 208 | mp_rep->addInputChangeWithNoise(input, m_Q, input_stamp, mp_lkf_vector[0]); 209 | m_sc = mp_rep->predictTo(predict_stamp); 210 | } else { 211 | mp_lkf->A = A; 212 | m_sc = mp_lkf->predict(m_sc, input, m_Q, dt); 213 | } 214 | /* mp_lkf->B = B; */ 215 | } 216 | catch (const std::exception &e) { 217 | // In case of error, alert the user 218 | ROS_ERROR("[HeadingEstimator]: LKF prediction step failed: %s", e.what()); 219 | } 220 | } 221 | return true; 222 | } 223 | 224 | //} 225 | 226 | /* //{ doPrediction() */ 227 | 228 | bool HeadingEstimator::doPrediction(const hdg_u_t &input, const ros::Time &input_stamp, const ros::Time &predict_stamp) { 229 | 230 | return doPrediction(input, m_dt, input_stamp, predict_stamp); 231 | } 232 | 233 | //} 234 | 235 | /* //{ doCorrection() */ 236 | 237 | bool HeadingEstimator::doCorrection(const double measurement, int measurement_type, const ros::Time &meas_stamp, const ros::Time &predict_stamp) { 238 | 239 | /* //{ sanity checks */ 240 | 241 | if (!m_is_initialized) 242 | return false; 243 | 244 | // Check for NaNs 245 | if (!std::isfinite(measurement)) { 246 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".doCorrection(const double measurement=" << measurement 247 | << ", int measurement_type=" << measurement_type << "): NaN detected in variable \"measurement\"." << std::endl; 248 | return false; 249 | } 250 | 251 | if (!std::isfinite(measurement_type)) { 252 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".doCorrection(const double &measurement=" << measurement 253 | << ", int measurement_type=" << measurement_type << "): NaN detected in variable \"measurement\"." << std::endl; 254 | return false; 255 | } 256 | 257 | // Check for valid value of measurement 258 | if (measurement_type > (int)m_fusing_measurement.size() || measurement_type < 0) { 259 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".doCorrection(const double &measurement=" << measurement 260 | << ", int measurement_type=" << measurement_type << "): invalid value of \"measurement_type\"." << std::endl; 261 | return false; 262 | } 263 | 264 | // Check whether the measurement type is fused by this estimator 265 | if (!m_fusing_measurement[measurement_type]) { 266 | return false; 267 | } 268 | 269 | //} 270 | 271 | // Prepare the measurement vector 272 | hdg_z_t z; 273 | z << measurement; 274 | 275 | hdg_R_t R; 276 | R << m_R_multi[measurement_type]; 277 | 278 | // Fuse the measurement 279 | std::scoped_lock lock(mutex_lkf); 280 | { 281 | 282 | try { 283 | if (m_use_repredictor) { 284 | mp_rep->addMeasurement(z, R, meas_stamp, mp_lkf_vector[measurement_type]); 285 | m_sc = mp_rep->predictTo(predict_stamp); 286 | } else { 287 | mp_lkf->H = m_H_multi[measurement_type]; 288 | m_sc = mp_lkf->correct(m_sc, z, R); 289 | } 290 | } 291 | catch (const std::exception &e) { 292 | // In case of error, alert the user 293 | ROS_ERROR("[HeadingEstimator]: LKF correction step failed: %s", e.what()); 294 | } 295 | } 296 | 297 | return true; 298 | } 299 | 300 | //} 301 | 302 | /* //{ getStates() */ 303 | 304 | bool HeadingEstimator::getStates(hdg_x_t &states) { 305 | 306 | /* //{ sanity checks */ 307 | 308 | if (!m_is_initialized) 309 | return false; 310 | 311 | //} 312 | 313 | std::scoped_lock lock(mutex_lkf); 314 | 315 | states = m_sc.x; 316 | 317 | return true; 318 | } 319 | 320 | //} 321 | 322 | /* //{ getState() */ 323 | 324 | bool HeadingEstimator::getState(int state_id, double &state) { 325 | 326 | /* //{ sanity checks */ 327 | 328 | if (!m_is_initialized) 329 | return false; 330 | 331 | // Check for NaNs 332 | if (!std::isfinite(state_id)) { 333 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".getState(int state_id=" << state_id << ", Eigen::VectorXd &state=" << state 334 | << "): NaN detected in variable \"state_id\"." << std::endl; 335 | return false; 336 | } 337 | 338 | // Check validity of state_id 339 | if (state_id < 0 || state_id > m_n_states - 1) { 340 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".getState(int state_id=" << state_id << ", Eigen::VectorXd &state=" << state 341 | << "): Invalid value of \"state_id\"." << std::endl; 342 | return false; 343 | } 344 | 345 | //} 346 | 347 | { 348 | std::scoped_lock lock(mutex_lkf); 349 | 350 | state = m_sc.x(state_id); 351 | } 352 | 353 | return true; 354 | } 355 | 356 | //} 357 | 358 | /* //{ getName() */ 359 | 360 | std::string HeadingEstimator::getName(void) { 361 | return m_estimator_name; 362 | } 363 | 364 | //} 365 | 366 | /* //{ setState() */ 367 | 368 | bool HeadingEstimator::setState(int state_id, const double state) { 369 | 370 | /* //{ sanity checks */ 371 | 372 | if (!m_is_initialized) 373 | return false; 374 | 375 | // Check for NaNs 376 | if (!std::isfinite(state)) { 377 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 378 | << "): NaN detected in variable \"state\"." << std::endl; 379 | return false; 380 | } 381 | 382 | if (!std::isfinite(state_id)) { 383 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 384 | << "): NaN detected in variable \"state_id\"." << std::endl; 385 | return false; 386 | } 387 | 388 | // Check validity of state_id 389 | if (state_id < 0 || state_id > m_n_states - 1) { 390 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 391 | << "): Invalid value of \"state_id\"." << std::endl; 392 | return false; 393 | } 394 | 395 | //} 396 | 397 | { 398 | std::scoped_lock lock(mutex_lkf); 399 | 400 | m_sc.x(state_id) = state; 401 | // reset repredictor 402 | if (m_use_repredictor) { 403 | const var_hdg_u_t u0 = hdg_u_t::Zero(); 404 | const ros::Time t0 = ros::Time(0); 405 | mp_rep = std::make_unique(m_sc.x, m_sc.P, u0, m_Q, t0, mp_lkf_vector.at(0), m_buf_sz); 406 | } 407 | } 408 | 409 | return true; 410 | } 411 | 412 | //} 413 | 414 | /* //{ setR() */ 415 | 416 | bool HeadingEstimator::setR(double cov, int measurement_type) { 417 | 418 | /* //{ sanity checks */ 419 | 420 | if (!m_is_initialized) 421 | return false; 422 | 423 | // Check for NaNs 424 | if (!std::isfinite(cov)) { 425 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type 426 | << "): NaN detected in variable \"cov\"." << std::endl; 427 | return false; 428 | } 429 | 430 | // Check for non-positive covariance 431 | if (cov <= 0) { 432 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type 433 | << "): \"cov\" should be > 0." << std::endl; 434 | return false; 435 | } 436 | 437 | // Check for invalid measurement type 438 | if (measurement_type > (int)m_fusing_measurement.size() || measurement_type < 0) { 439 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type 440 | << "): invalid value of \"measurement_type\"." << std::endl; 441 | return false; 442 | } 443 | 444 | //} 445 | 446 | double old_cov = m_R_multi[measurement_type](0, 0); 447 | 448 | { 449 | std::scoped_lock lock(mutex_lkf); 450 | 451 | m_R_multi[measurement_type](0, 0) = cov; 452 | } 453 | 454 | std::cout << "[HeadingEstimator]: " << m_estimator_name << ".setQ(double cov=" << cov << ", int measurement_type=" << measurement_type << ")" 455 | << " Changed covariance from: " << old_cov << " to: " << m_R_multi[measurement_type](0, 0) << std::endl; 456 | 457 | return true; 458 | } 459 | 460 | //} 461 | 462 | /* //{ getR() */ 463 | 464 | bool HeadingEstimator::getR(double &cov, int measurement_type) { 465 | 466 | /* //{ sanity checks */ 467 | 468 | if (!m_is_initialized) 469 | return false; 470 | 471 | // Check for NaNs 472 | if (!std::isfinite(measurement_type)) { 473 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".getCovariance(int measurement_type=" << measurement_type 474 | << "): NaN detected in variable \"measurement_type\"." << std::endl; 475 | return false; 476 | } 477 | 478 | // Check for invalid measurement type 479 | if (measurement_type > (int)m_fusing_measurement.size() || measurement_type < 0) { 480 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".getCovariance(int measurement_type=" << measurement_type 481 | << "): invalid value of \"measurement_type\"." << std::endl; 482 | return false; 483 | } 484 | 485 | //} 486 | 487 | { 488 | std::scoped_lock lock(mutex_lkf); 489 | 490 | cov = m_R_multi[measurement_type](0, 0); 491 | } 492 | 493 | return true; 494 | } 495 | 496 | //} 497 | 498 | /* //{ getCovariance() */ 499 | 500 | bool HeadingEstimator::getCovariance(hdg_P_t &cov) { 501 | 502 | /* //{ sanity checks */ 503 | 504 | if (!m_is_initialized) 505 | return false; 506 | 507 | //} 508 | 509 | { 510 | std::scoped_lock lock(mutex_lkf); 511 | 512 | cov = m_sc.P; 513 | } 514 | 515 | return true; 516 | } 517 | 518 | //} 519 | 520 | /* //{ setCovariance() */ 521 | 522 | bool HeadingEstimator::setCovariance(const hdg_P_t &cov) { 523 | 524 | /* //{ sanity checks */ 525 | 526 | if (!m_is_initialized) 527 | return false; 528 | 529 | // Check size of measurement 530 | if (cov.rows() != m_n_states || cov.cols() != m_n_states) { 531 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setCovariance(const Eigen::MatrixXd &cov=" << cov << "): wrong size of \"cov\". Should be: (" 532 | << m_n_states << "," << m_n_states << ") is: (" << cov.rows() << "," << cov.cols() << ")" << std::endl; 533 | return false; 534 | } 535 | 536 | // Check for NaNs 537 | for (int i = 0; i < m_n_states; i++) { 538 | if (!std::isfinite(cov(i, i))) { 539 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".setCovariance(const Eigen::MatrixXd &cov=" << cov << "): NaN detected in variable \"cov(" 540 | << i << "," << i << ")\"." << std::endl; 541 | return false; 542 | } 543 | } 544 | 545 | //} 546 | 547 | // Set the covariance 548 | { 549 | std::scoped_lock lock(mutex_lkf); 550 | 551 | m_sc.P = cov; 552 | // reset repredictor 553 | if (m_use_repredictor) { 554 | const var_hdg_u_t u0 = hdg_u_t::Zero(); 555 | const ros::Time t0 = ros::Time(0); 556 | mp_rep = std::make_unique(m_sc.x, m_sc.P, u0, m_Q, t0, mp_lkf_vector.at(0), m_buf_sz); 557 | } 558 | } 559 | 560 | return true; 561 | } 562 | //} 563 | 564 | /* //{ reset() */ 565 | 566 | bool HeadingEstimator::reset(const hdg_x_t &states) { 567 | 568 | /* //{ sanity checks */ 569 | 570 | if (!m_is_initialized) 571 | return false; 572 | // Check size of states 573 | if ((int)states.rows() != m_n_states) { 574 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".reset(const Eigen::MatrixXd &states=" // << states 575 | << "): wrong size of \"states.rows()\". Should be: " << m_n_states << " is:" << states.rows() << std::endl; 576 | return false; 577 | } 578 | 579 | if (states.cols() != 1) { 580 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".reset(const Eigen::MatrixXd &states=" // << states 581 | << "): wrong size of \"states.cols()\". Should be: " << 1 << " is:" << states.cols() << std::endl; 582 | return false; 583 | } 584 | 585 | // Check for NaNs 586 | for (int i = 0; i < states.rows(); i++) { 587 | for (int j = 0; j < states.cols(); j++) { 588 | if (!std::isfinite(states(i, j))) { 589 | std::cerr << "[HeadingEstimator]: " << m_estimator_name << ".reset(const Eigen::MatrixXd &states=" // << states 590 | << "): NaN detected in variable \"states(" << i << ", " << j << ")\"." << std::endl; 591 | return false; 592 | } 593 | } 594 | } 595 | 596 | //} 597 | 598 | { 599 | std::scoped_lock lock(mutex_lkf); 600 | 601 | m_sc.x = (states.col(0)); 602 | if (m_use_repredictor) { 603 | const var_hdg_u_t u0 = hdg_u_t::Zero(); 604 | const ros::Time t0 = ros::Time(0); 605 | mp_rep = std::make_unique(m_sc.x, m_sc.P, u0, m_Q, t0, mp_lkf_vector.at(0), m_buf_sz); 606 | } 607 | } 608 | 609 | return true; 610 | } 611 | 612 | //} 613 | 614 | } // namespace mrs_uav_odometry 615 | -------------------------------------------------------------------------------- /src/StateEstimator.cpp: -------------------------------------------------------------------------------- 1 | #include "StateEstimator.h" 2 | 3 | namespace mrs_uav_odometry 4 | { 5 | 6 | /* //{ StateEstimator() */ 7 | 8 | // clang-format off 9 | StateEstimator::StateEstimator( 10 | const std::string &estimator_name, 11 | const std::vector &fusing_measurement, 12 | const LatMat &Q, 13 | const std::vector &H, 14 | const std::vector &R_arr, 15 | const bool use_repredictor) 16 | : 17 | m_estimator_name(estimator_name), 18 | m_fusing_measurement(fusing_measurement), 19 | m_Q(Q), 20 | m_H(H), 21 | m_R_arr(R_arr), 22 | m_use_repredictor(use_repredictor) 23 | { 24 | 25 | // clang-format on 26 | 27 | // Number of states 28 | m_n_states = sc_x.x.size(); 29 | 30 | // Number of measurement types 31 | m_n_measurement_types = m_fusing_measurement.size(); 32 | 33 | /* //{ sanity checks */ 34 | 35 | // Check size of m_Q 36 | if (m_Q.rows() != m_n_states) { 37 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".StateEstimator()" 38 | << "): wrong size of \"Q.rows()\". Should be: " << m_n_states << " is:" << m_Q.rows() << std::endl; 39 | return; 40 | } 41 | 42 | if (m_Q.cols() != m_n_states) { 43 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".StateEstimator()" 44 | << "): wrong size of \"Q.cols()\". Should be: " << m_n_states << " is:" << m_Q.cols() << std::endl; 45 | return; 46 | } 47 | 48 | // Check size of m_R_arr 49 | if (m_R_arr.size() != m_n_measurement_types) { 50 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".StateEstimator()" 51 | << "): wrong size of \"m_R_arr\". Should be: " << m_n_measurement_types << " is:" << m_R_arr.size() << std::endl; 52 | return; 53 | } 54 | 55 | // Check size of m_R_arr elements 56 | for (size_t i = 0; i < m_R_arr.size(); i++) { 57 | if (m_R_arr[i].rows() != 1 || m_R_arr[i].cols() != 1) { 58 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".StateEstimator()" 59 | << "): wrong size of \"m_R_arr[" << i << "]\". Should be: (1, " << 1 << ") is: (" << m_R_arr[i].rows() << ", " << m_R_arr[i].cols() << ")" 60 | << std::endl; 61 | return; 62 | } 63 | } 64 | 65 | 66 | //} 67 | 68 | // Initialize all states to 0 69 | const x_t x0 = x_t::Zero(); 70 | P_t P_tmp = P_t::Identity(); 71 | const P_t P0 = 1000.0 * P_tmp; 72 | const statecov_t scx0({x0, P0}); 73 | const statecov_t scy0({x0, P0}); 74 | sc_x = scx0; 75 | sc_y = scy0; 76 | const u_t u0 = u_t::Zero(); 77 | const ros::Time t0 = ros::Time(0); 78 | 79 | if (m_use_repredictor) { 80 | std::cout << "[StateEstimator]: Using repredictor in " << m_estimator_name.c_str() << " estimator." << std::endl; 81 | // Initialize separate LKF models for each H matrix 82 | for (size_t i = 0; i < m_H.size(); i++) { 83 | std::vector curr_H{m_H[i]}; 84 | mp_lkf_vector.push_back(std::make_shared(curr_H, 0.01)); 85 | } 86 | // Initialize repredictor 87 | mp_rep_x = std::make_unique(x0, P0, u0, Q, t0, mp_lkf_vector.at(0), m_buf_sz); 88 | mp_rep_y = std::make_unique(x0, P0, u0, Q, t0, mp_lkf_vector.at(0), m_buf_sz); 89 | } else { 90 | // Initialize a single LKF 91 | mp_lkf_x = std::make_unique(m_H, 0.01); 92 | mp_lkf_y = std::make_unique(m_H, 0.01); 93 | } 94 | 95 | std::cout << "[StateEstimator]: New StateEstimator initialized " << std::endl; 96 | std::cout << "name: " << m_estimator_name << std::endl; 97 | std::cout << " fusing measurements: " << std::endl; 98 | for (size_t i = 0; i < m_fusing_measurement.size(); i++) { 99 | std::cout << m_fusing_measurement[i] << " "; 100 | } 101 | std::cout << std::endl << " R_arr: " << std::endl; 102 | for (size_t i = 0; i < m_R_arr.size(); i++) { 103 | std::cout << m_R_arr[i] << std::endl; 104 | } 105 | std::cout << std::endl << " H_arr: " << std::endl; 106 | for (size_t i = 0; i < m_H.size(); i++) { 107 | std::cout << m_H[i] << std::endl; 108 | } 109 | std::cout << std::endl << " Q: " << std::endl << m_Q << std::endl; 110 | 111 | m_is_initialized = true; 112 | } 113 | 114 | //} 115 | 116 | /* //{ doPrediction() */ 117 | 118 | bool StateEstimator::doPrediction(const Vec2 &input, double dt, const ros::Time &input_stamp, const ros::Time &predict_stamp) { 119 | 120 | /* //{ sanity checks */ 121 | 122 | if (!m_is_initialized) 123 | return false; 124 | 125 | // Check size of input 126 | if (input.size() != 2) { 127 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input << ", double dt=" << dt 128 | << "): wrong size of \"input\". Should be: " << 2 << " is:" << input.size() << std::endl; 129 | return false; 130 | } 131 | 132 | // Check for NaNs 133 | if (!std::isfinite(input(0))) { 134 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input << ", double dt=" << dt 135 | << "): NaN detected in variable \"input(0)\"." << std::endl; 136 | return false; 137 | } 138 | 139 | if (!std::isfinite(input(1))) { 140 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input << ", double dt=" << dt 141 | << "): NaN detected in variable \"input(1)\"." << std::endl; 142 | return false; 143 | } 144 | 145 | if (!std::isfinite(dt)) { 146 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input << ", double dt=" << dt 147 | << "): NaN detected in variable \"dt\"." << std::endl; 148 | return false; 149 | } 150 | 151 | // Check for non-positive dt 152 | if (dt <= 0) { 153 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doPrediction(const Eigen::VectorXd &input=" << input << ", double dt=" << dt 154 | << "): \"dt\" should be > 0." << std::endl; 155 | return false; 156 | } 157 | 158 | //} 159 | 160 | /* std::cout << "[StateEstimator]: " << m_estimator_name << " fusing input: " << input << " with time step: " << dt << std::endl; */ 161 | 162 | u_t u_x; 163 | u_t u_y; 164 | 165 | u_x << input(0); 166 | u_y << input(1); 167 | 168 | /* LatMat newA = m_A; */ 169 | /* newA(0, 1) = dt; */ 170 | /* newA(1, 2) = dt; */ 171 | /* newA(1, 3) = std::pow(dt,2); */ 172 | 173 | { 174 | std::scoped_lock lock(mutex_lkf); 175 | 176 | /* mp_lkf_x->setA(newA); */ 177 | /* mp_lkf_x->setInput(input_vec_x); */ 178 | /* mp_lkf_x->iterateWithoutCorrection(); */ 179 | /* mp_lkf_y->setA(newA); */ 180 | /* mp_lkf_y->setInput(input_vec_y); */ 181 | /* mp_lkf_y->iterateWithoutCorrection(); */ 182 | try { 183 | // Apply the prediction step 184 | if (m_use_repredictor) { 185 | mp_rep_x->addInputChangeWithNoise(u_x, m_Q, input_stamp, mp_lkf_vector[0]); 186 | sc_x = mp_rep_x->predictTo(predict_stamp); 187 | mp_rep_y->addInputChangeWithNoise(u_y, m_Q, input_stamp, mp_lkf_vector[0]); 188 | sc_y = mp_rep_y->predictTo(predict_stamp); 189 | } else { 190 | sc_x = mp_lkf_x->predict(sc_x, u_x, m_Q, dt); 191 | sc_y = mp_lkf_y->predict(sc_y, u_y, m_Q, dt); 192 | } 193 | } 194 | catch (const std::exception &e) { 195 | // In case of error, alert the user 196 | ROS_ERROR("[Odometry]: LKF prediction step failed: %s", e.what()); 197 | } 198 | } 199 | 200 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: prediction step" << std::endl); */ 201 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: input x:" << u_x << std::endl << "y: " << u_y << std::endl << std::endl); */ 202 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: m_Q:" << m_Q << std::endl << std::endl); */ 203 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: dt:" << dt << std::endl << std::endl); */ 204 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: state x:" << sc_x.x << std::endl << "y: " << sc_y.x << std::endl << std::endl); */ 205 | 206 | return true; 207 | } 208 | 209 | //} 210 | 211 | /* //{ doCorrection() */ 212 | 213 | bool StateEstimator::doCorrection(const Vec2 &measurement, int measurement_type, const ros::Time &meas_stamp, const ros::Time &predict_stamp) { 214 | 215 | /* //{ sanity checks */ 216 | 217 | if (!m_is_initialized) 218 | return false; 219 | 220 | // Check size of measurement 221 | if (measurement.size() != 2) { 222 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doCorrection(const Eigen::VectorXd &measurement=" << measurement 223 | << ", int measurement_type=" << measurement_type << "): wrong size of \"input\". Should be: " << 2 << " is:" << measurement.size() << std::endl; 224 | return false; 225 | } 226 | 227 | // Check for NaNs 228 | if (!std::isfinite(measurement(0))) { 229 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doCorrection(const Eigen::VectorXd &measurement=" << measurement 230 | << ", int measurement_type=" << measurement_type << "): NaN detected in variable \"measurement(0)\"." << std::endl; 231 | return false; 232 | } 233 | 234 | if (!std::isfinite(measurement(1))) { 235 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doCorrection(const Eigen::VectorXd &measurement=" << measurement 236 | << ", int measurement_type=" << measurement_type << "): NaN detected in variable \"measurement(0)\"." << std::endl; 237 | return false; 238 | } 239 | 240 | if (!std::isfinite(measurement_type)) { 241 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doCorrection(const Eigen::VectorXd &measurement=" << measurement 242 | << ", int measurement_type=" << measurement_type << "): NaN detected in variable \"measurement(0)\"." << std::endl; 243 | return false; 244 | } 245 | 246 | // Check for valid value of measurement 247 | if (measurement_type > (int)m_fusing_measurement.size() || measurement_type < 0) { 248 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".doCorrection(const Eigen::VectorXd &measurement=" << measurement 249 | << ", int measurement_type=" << measurement_type << "): invalid value of \"measurement_type\"." << std::endl; 250 | return false; 251 | } 252 | 253 | // Check whether the measurement type is fused by this estimator 254 | if (!m_fusing_measurement[measurement_type]) { 255 | return false; 256 | } 257 | 258 | //} 259 | 260 | z_t z_x; 261 | z_t z_y; 262 | R_t R; 263 | 264 | z_x << measurement(0); 265 | z_y << measurement(1); 266 | R << m_R_arr[measurement_type]; 267 | 268 | { 269 | std::scoped_lock lock(mutex_lkf); 270 | 271 | /* mp_lkf_x->setP(m_P_arr[measurement_type]); */ 272 | /* mp_lkf_x->setMeasurement(mes_vec_x, m_R_arr[measurement_type]); */ 273 | /* mp_lkf_x->doCorrection(); */ 274 | /* mp_lkf_y->setP(m_P_arr[measurement_type]); */ 275 | /* mp_lkf_y->setMeasurement(mes_vec_y, m_R_arr[measurement_type]); */ 276 | /* mp_lkf_y->doCorrection(); */ 277 | 278 | try { 279 | if (m_use_repredictor) { 280 | mp_rep_x->addMeasurement(z_x, R, meas_stamp, mp_lkf_vector[measurement_type]); 281 | sc_x = mp_rep_x->predictTo(predict_stamp); 282 | mp_rep_y->addMeasurement(z_y, R, meas_stamp, mp_lkf_vector[measurement_type]); 283 | sc_y = mp_rep_y->predictTo(predict_stamp); 284 | } else { 285 | sc_x = mp_lkf_x->correct(sc_x, z_x, R, measurement_type); 286 | sc_y = mp_lkf_y->correct(sc_y, z_y, R, measurement_type); 287 | } 288 | } 289 | catch (const std::exception &e) { 290 | // In case of error, alert the user 291 | ROS_ERROR("[Odometry]: LKF correction step failed: %s", e.what()); 292 | } 293 | } 294 | 295 | /* if (measurement_type==1) { */ 296 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: correction type: " << measurement_type << std::endl); */ 297 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: corr x:" << z_x << std::endl << "y: " << z_y << std::endl << std::endl); */ 298 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: R: " << R << std::endl << std::endl); */ 299 | /* ROS_INFO_STREAM_THROTTLE(1.0, "[StateEstimator]: state x:" << sc_x.x << std::endl << "y: " << sc_y.x << std::endl << std::endl); */ 300 | /* } */ 301 | 302 | return true; 303 | } 304 | 305 | //} 306 | 307 | /* //{ getStates() */ 308 | 309 | bool StateEstimator::getStates(LatState2D &states) { 310 | 311 | /* //{ sanity checks */ 312 | 313 | if (!m_is_initialized) 314 | return false; 315 | 316 | //} 317 | 318 | std::scoped_lock lock(mutex_lkf); 319 | 320 | states.col(0) = sc_x.x; 321 | states.col(1) = sc_y.x; 322 | 323 | return true; 324 | } 325 | 326 | //} 327 | 328 | /* //{ getState() */ 329 | 330 | bool StateEstimator::getState(int state_id, Vec2 &state) { 331 | 332 | /* //{ sanity checks */ 333 | 334 | if (!m_is_initialized) 335 | return false; 336 | 337 | // Check for NaNs 338 | if (!std::isfinite(state_id)) { 339 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".getState(int state_id=" << state_id << "): NaN detected in variable \"state_id\"." << std::endl; 340 | return false; 341 | } 342 | 343 | // Check validity of state_id 344 | if (state_id < 0 || state_id > m_n_states - 1) { 345 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".getState(int state_id=" << state_id << "): Invalid value of \"state_id\"." << std::endl; 346 | return false; 347 | } 348 | 349 | //} 350 | 351 | { 352 | std::scoped_lock lock(mutex_lkf); 353 | 354 | /* std::cout << "[StateEstimator]: " << m_estimator_name << " getting value: " << mp_lkf_x->getState(state_id) << " of state: " << state_id << std::endl; 355 | */ 356 | state(0) = sc_x.x(state_id); 357 | state(1) = sc_y.x(state_id); 358 | } 359 | 360 | return true; 361 | } 362 | 363 | //} 364 | 365 | /* //{ getName() */ 366 | 367 | std::string StateEstimator::getName(void) { 368 | return m_estimator_name; 369 | } 370 | 371 | //} 372 | 373 | /* //{ setState() */ 374 | 375 | bool StateEstimator::setState(int state_id, const Vec2 &state) { 376 | 377 | /* //{ sanity checks */ 378 | 379 | if (!m_is_initialized) { 380 | /* ROS_WARN("[%s]: Lateral estimator %s method setState() called before init.", ros::this_node::getName().c_str(), m_estimator_name.c_str()); */ 381 | return false; 382 | } 383 | 384 | // Check the size of state 385 | if (state.size() != 2) { 386 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 387 | << "): wrong size of \"state.size()\". Should be: " << 2 << " is:" << state.size() << std::endl; 388 | return false; 389 | } 390 | 391 | // Check for NaNs 392 | if (!std::isfinite(state(0))) { 393 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 394 | << "): NaN detected in variable \"state(0)\"." << std::endl; 395 | return false; 396 | } 397 | 398 | if (!std::isfinite(state(1))) { 399 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 400 | << "): NaN detected in variable \"state(1)\"." << std::endl; 401 | return false; 402 | } 403 | 404 | if (!std::isfinite(state_id)) { 405 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 406 | << "): NaN detected in variable \"state_id\"." << std::endl; 407 | return false; 408 | } 409 | 410 | // Check validity of state_id 411 | if (state_id < 0 || state_id > m_n_states - 1) { 412 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setState(int state_id=" << state_id << ", const Eigen::VectorXd &state=" << state 413 | << "): Invalid value of \"state_id\"." << std::endl; 414 | return false; 415 | } 416 | 417 | //} 418 | 419 | { 420 | std::scoped_lock lock(mutex_lkf); 421 | 422 | sc_x.x(state_id) = state(0); 423 | sc_y.x(state_id) = state(1); 424 | } 425 | /* ROS_INFO("[%s]: Set state %d of %s to x: %f y: %f. State after x: %f y: %f", ros::this_node::getName().c_str(), state_id, m_estimator_name.c_str(), 426 | * state(0), state(1), sc_x.x(state_id), sc_y.x(state_id)); */ 427 | 428 | return true; 429 | } 430 | 431 | //} 432 | 433 | /* //{ setStates() */ 434 | 435 | bool StateEstimator::setStates(LatState2D &states) { 436 | 437 | /* //{ sanity checks */ 438 | 439 | if (!m_is_initialized) 440 | return false; 441 | 442 | // Check size of states 443 | if (states.rows() != m_n_states) { 444 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setStates()" 445 | << ": wrong size of \"states\". Should be: " << m_n_states << " is: " << states.cols() << std::endl; 446 | return false; 447 | } 448 | 449 | if (states.cols() != 2) { 450 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setStates()" 451 | << ": wrong size of \"states\". Should be: " << 2 << " is:" << states.rows() << std::endl; 452 | return false; 453 | } 454 | //} 455 | 456 | { 457 | std::scoped_lock lock(mutex_lkf); 458 | sc_x.x = states.col(0); 459 | sc_y.x = states.col(1); 460 | } 461 | 462 | return true; 463 | } 464 | 465 | //} 466 | 467 | /* //{ setR() */ 468 | 469 | bool StateEstimator::setR(double cov, int measurement_type) { 470 | 471 | /* //{ sanity checks */ 472 | 473 | if (!m_is_initialized) 474 | return false; 475 | 476 | // Check for NaNs 477 | if (!std::isfinite(cov)) { 478 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type 479 | << "): NaN detected in variable \"cov\"." << std::endl; 480 | return false; 481 | } 482 | 483 | // Check for non-positive covariance 484 | if (cov <= 0) { 485 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type 486 | << "): \"cov\" should be > 0." << std::endl; 487 | return false; 488 | } 489 | 490 | // Check for invalid measurement type 491 | if (measurement_type > (int)m_fusing_measurement.size() || measurement_type < 0) { 492 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type 493 | << "): invalid value of \"measurement_type\"." << std::endl; 494 | return false; 495 | } 496 | 497 | //} 498 | 499 | /* double old_cov = m_R_arr[measurement_type](0, 0); */ 500 | 501 | { 502 | std::scoped_lock lock(mutex_lkf); 503 | 504 | m_R_arr[measurement_type](0, 0) = cov; 505 | } 506 | 507 | /* std::cout << "[StateEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type << ")" */ 508 | /* << " Changed covariance from: " << old_cov << " to: " << m_R_arr[measurement_type](0, 0) << std::endl; */ 509 | 510 | return true; 511 | } 512 | 513 | //} 514 | 515 | /* //{ getR() */ 516 | 517 | bool StateEstimator::getR(double &cov, int measurement_type) { 518 | 519 | /* //{ sanity checks */ 520 | 521 | if (!m_is_initialized) 522 | return false; 523 | 524 | // Check for NaNs 525 | if (!std::isfinite(measurement_type)) { 526 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".getCovariance(int measurement_type=" << measurement_type 527 | << "): NaN detected in variable \"measurement_type\"." << std::endl; 528 | return false; 529 | } 530 | 531 | // Check for invalid measurement type 532 | if (measurement_type > (int)m_fusing_measurement.size() || measurement_type < 0) { 533 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".getCovariance(int measurement_type=" << measurement_type 534 | << "): invalid value of \"measurement_type\"." << std::endl; 535 | return false; 536 | } 537 | 538 | //} 539 | 540 | { 541 | std::scoped_lock lock(mutex_lkf); 542 | 543 | cov = m_R_arr[measurement_type](0, 0); 544 | } 545 | 546 | return true; 547 | } 548 | 549 | //} 550 | 551 | /* //{ getQ() */ 552 | 553 | bool StateEstimator::getQ(double &cov, const Eigen::Vector2i &idx) { 554 | 555 | /* //{ sanity checks */ 556 | 557 | if (!m_is_initialized) 558 | return false; 559 | 560 | // Check for index validity 561 | if (idx(0) > m_n_states || idx(1) > m_n_states || idx(0) < 0 || idx(1) < 0) { 562 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setR(double cov=" << cov << ", int" 563 | << "): \"idx\" should be < " << m_n_states << "." << std::endl; 564 | return false; 565 | } 566 | 567 | //} 568 | 569 | { 570 | std::scoped_lock lock(mutex_lkf); 571 | 572 | cov = m_Q(idx(0), idx(1)); 573 | } 574 | 575 | return true; 576 | } 577 | 578 | //} 579 | 580 | /* //{ setQ() */ 581 | 582 | bool StateEstimator::setQ(double cov, const Eigen::Vector2i &idx) { 583 | 584 | /* //{ sanity checks */ 585 | 586 | if (!m_is_initialized) 587 | return false; 588 | 589 | // Check for NaNs 590 | if (!std::isfinite(cov)) { 591 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setR(double cov=" << cov << ", int" 592 | << "): NaN detected in variable \"cov\"." << std::endl; 593 | return false; 594 | } 595 | 596 | // Check for non-positive covariance 597 | if (cov <= 0) { 598 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setR(double cov=" << cov << ", int" 599 | << "): \"cov\" should be > 0." << std::endl; 600 | return false; 601 | } 602 | 603 | // Check for index validity 604 | if (idx(0) > m_n_states || idx(1) > m_n_states || idx(0) < 0 || idx(1) < 0) { 605 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".setR(double cov=" << cov << ", int" 606 | << "): \"idx\" should be < " << m_n_states << "." << std::endl; 607 | return false; 608 | } 609 | 610 | //} 611 | 612 | { 613 | std::scoped_lock lock(mutex_lkf); 614 | 615 | m_Q(idx(0), idx(1)) = cov; 616 | } 617 | 618 | /* std::cout << "[StateEstimator]: " << m_estimator_name << ".setCovariance(double cov=" << cov << ", int measurement_type=" << measurement_type << ")" */ 619 | /* << " Changed covariance from: " << old_cov << " to: " << m_Q_arr[measurement_type](0, 0) << std::endl; */ 620 | 621 | return true; 622 | } 623 | 624 | //} 625 | 626 | /* //{ reset() */ 627 | 628 | bool StateEstimator::reset(const LatState2D &states) { 629 | 630 | /* //{ sanity checks */ 631 | 632 | if (!m_is_initialized) 633 | return false; 634 | // Check size of states 635 | if ((int)states.rows() != m_n_states) { 636 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".reset(const Eigen::MatrixXd &states=" // << states 637 | << "): wrong size of \"states.rows()\". Should be: " << m_n_states << " is:" << states.rows() << std::endl; 638 | return false; 639 | } 640 | 641 | if (states.cols() != 2) { 642 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".reset(const Eigen::MatrixXd &states=" // << states 643 | << "): wrong size of \"states.cols()\". Should be: " << 2 << " is:" << states.cols() << std::endl; 644 | return false; 645 | } 646 | 647 | // Check for NaNs 648 | for (int i = 0; i < states.rows(); i++) { 649 | for (int j = 0; j < states.cols(); j++) { 650 | if (!std::isfinite(states(i, j))) { 651 | std::cerr << "[StateEstimator]: " << m_estimator_name << ".reset(const Eigen::MatrixXd &states=" // << states 652 | << "): NaN detected in variable \"states(" << i << ", " << j << ")\"." << std::endl; 653 | return false; 654 | } 655 | } 656 | } 657 | 658 | //} 659 | 660 | { 661 | std::scoped_lock lock(mutex_lkf); 662 | 663 | sc_x.x = states.col(0); 664 | sc_y.x = states.col(1); 665 | 666 | if (m_use_repredictor) { 667 | const u_t u0 = u_t::Zero(); 668 | const ros::Time t0 = ros::Time(0); 669 | mp_rep_x = std::make_unique(sc_x.x, sc_x.P, u0, m_Q, t0, mp_lkf_vector.at(0), m_buf_sz); 670 | mp_rep_y = std::make_unique(sc_y.x, sc_y.P, u0, m_Q, t0, mp_lkf_vector.at(0), m_buf_sz); 671 | } 672 | } 673 | 674 | return true; 675 | } 676 | 677 | //} 678 | 679 | } // namespace mrs_uav_odometry 680 | -------------------------------------------------------------------------------- /src/rtk_republisher.cpp: -------------------------------------------------------------------------------- 1 | /* includes //{ */ 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | //} 25 | 26 | #define btoa(x) ((x) ? "true" : "false") 27 | 28 | namespace mrs_uav_odometry 29 | { 30 | 31 | /* class RtkRepublisher //{ */ 32 | 33 | class RtkRepublisher : public nodelet::Nodelet { 34 | 35 | public: 36 | virtual void onInit(); 37 | 38 | private: 39 | ros::NodeHandle nh_; 40 | bool is_initialized_ = false; 41 | 42 | private: 43 | // subscribers and publishers 44 | ros::Subscriber subscriber_global_odom_; 45 | ros::Subscriber subscriber_tersus_; 46 | ros::Publisher rtk_publisher_; 47 | ros::Publisher odom_publisher; 48 | ros::Publisher pose_publisher_; 49 | ros::ServiceServer service_server_jump_emulation_; 50 | ros::ServiceServer service_server_random_jumps_; 51 | 52 | // publisher rate 53 | int _rate_; 54 | 55 | double _offset_x_, _offset_y_; 56 | 57 | double jump_offset_, jump_hdg_offset_; 58 | 59 | // simulation of RTK signal degradation 60 | bool add_random_jumps_; 61 | bool random_jump_active_; 62 | double random_jump_; 63 | int until_next_jump_; 64 | int until_jump_end_; 65 | double jump_amplitude_; 66 | 67 | mrs_msgs::RtkFixType fix_type_; 68 | 69 | // mutex for locking the position info 70 | std::mutex mutex_odom_; 71 | std::mutex mutex_tersus_; 72 | 73 | ros::Timer timer_main_; 74 | 75 | // global odometry from gazebo 76 | nav_msgs::Odometry odom_; 77 | bool got_odom_ = false; 78 | 79 | // republished pose message 80 | geometry_msgs::PoseWithCovarianceStamped pose_msg_out_; 81 | 82 | // rtk message from tersus_ 83 | mrs_msgs::Bestpos tersus_; 84 | bool got_tersus_ = false; 85 | 86 | private: 87 | void callbackOdometry(const nav_msgs::OdometryConstPtr &msg); 88 | void callbackTersus(const mrs_msgs::BestposConstPtr &msg); 89 | bool emulateJump([[maybe_unused]] std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); 90 | bool toggleRandomJumps([[maybe_unused]] std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); 91 | void timerMain(const ros::TimerEvent &event); 92 | 93 | private: 94 | mrs_lib::Profiler *profiler_; 95 | bool profiler_enabled_ = false; 96 | }; 97 | 98 | //} 99 | 100 | /* onInit() //{ */ 101 | 102 | void RtkRepublisher::onInit() { 103 | 104 | ros::NodeHandle nh_ = nodelet::Nodelet::getMTPrivateNodeHandle(); 105 | 106 | mrs_lib::ParamLoader param_loader(nh_, "RtkRepublisher"); 107 | 108 | param_loader.loadParam("enable_profiler", profiler_enabled_); 109 | 110 | param_loader.loadParam("rate", _rate_); 111 | param_loader.loadParam("offset_x", _offset_x_); 112 | param_loader.loadParam("offset_y", _offset_y_); 113 | 114 | if (!param_loader.loadedSuccessfully()) { 115 | ROS_ERROR("[RtkRepublisher]: could not load all parameters!"); 116 | ros::shutdown(); 117 | } 118 | 119 | // | ----------------------- subscribers ---------------------- | 120 | 121 | subscriber_global_odom_ = nh_.subscribe("odom_in", 1, &RtkRepublisher::callbackOdometry, this, ros::TransportHints().tcpNoDelay()); 122 | subscriber_tersus_ = nh_.subscribe("tersus_in", 1, &RtkRepublisher::callbackTersus, this, ros::TransportHints().tcpNoDelay()); 123 | 124 | // | ----------------------- publishers ----------------------- | 125 | 126 | rtk_publisher_ = nh_.advertise("rtk_out", 1); 127 | pose_publisher_ = nh_.advertise("pose_out", 1); 128 | 129 | // | ------------------------- timers ------------------------- | 130 | 131 | timer_main_ = nh_.createTimer(ros::Rate(_rate_), &RtkRepublisher::timerMain, this); 132 | 133 | // | ------------------------ services ------------------------ | 134 | 135 | service_server_jump_emulation_ = nh_.advertiseService("emulate_jump", &RtkRepublisher::emulateJump, this); 136 | service_server_random_jumps_ = nh_.advertiseService("toggle_random_jumps", &RtkRepublisher::toggleRandomJumps, this); 137 | 138 | // | ------------------------ profiler ------------------------ | 139 | 140 | profiler_ = new mrs_lib::Profiler(nh_, "RtkRepublisher", profiler_enabled_); 141 | 142 | // | ----------------------- finish init ---------------------- | 143 | 144 | jump_offset_ = 0.0; 145 | jump_hdg_offset_ = 0.0; 146 | 147 | add_random_jumps_ = false; 148 | random_jump_active_ = false; 149 | until_next_jump_ = 0.0; 150 | until_jump_end_ = 0.0; 151 | random_jump_ = 0.0; 152 | 153 | fix_type_.fix_type = mrs_msgs::RtkFixType::RTK_FIX; 154 | 155 | is_initialized_ = true; 156 | 157 | ROS_INFO("[RtkRepublisher]: initialized"); 158 | } 159 | 160 | //} 161 | 162 | // -------------------------------------------------------------- 163 | // | callbacks | 164 | // -------------------------------------------------------------- 165 | 166 | /* callbackOdometry() //{ */ 167 | 168 | void RtkRepublisher::callbackOdometry(const nav_msgs::OdometryConstPtr &msg) { 169 | 170 | if (!is_initialized_) 171 | return; 172 | 173 | mrs_lib::Routine profiler_routine = profiler_->createRoutine("callbackOdometry"); 174 | 175 | got_odom_ = true; 176 | 177 | { 178 | std::scoped_lock lock(mutex_odom_); 179 | 180 | odom_ = *msg; 181 | } 182 | } 183 | 184 | //} 185 | 186 | /* callbackTersus() //{ */ 187 | 188 | void RtkRepublisher::callbackTersus(const mrs_msgs::BestposConstPtr &msg) { 189 | 190 | if (!is_initialized_) 191 | return; 192 | 193 | mrs_lib::Routine profiler_routine = profiler_->createRoutine("callbackTersus"); 194 | 195 | { 196 | std::scoped_lock lock(mutex_tersus_); 197 | 198 | tersus_ = *msg; 199 | } 200 | mrs_msgs::RtkGps rtk_msg_out; 201 | 202 | rtk_msg_out.header.stamp = ros::Time::now(); 203 | rtk_msg_out.header.frame_id = "gps"; 204 | 205 | // copy the position 206 | { 207 | std::scoped_lock lock(mutex_tersus_); 208 | 209 | rtk_msg_out.gps.latitude = tersus_.latitude; 210 | rtk_msg_out.gps.longitude = tersus_.longitude; 211 | rtk_msg_out.gps.altitude = tersus_.height; 212 | rtk_msg_out.gps.covariance[0] = std::pow(tersus_.latitude_std, 2); 213 | rtk_msg_out.gps.covariance[4] = std::pow(tersus_.longitude_std, 2); 214 | rtk_msg_out.gps.covariance[8] = std::pow(tersus_.height_std, 2); 215 | 216 | if (tersus_.position_type == "L1_INT" || tersus_.position_type == "NARROW_INT" || tersus_.position_type == "WIDE_INT") { 217 | 218 | rtk_msg_out.fix_type.fix_type = mrs_msgs::RtkFixType::RTK_FIX; 219 | 220 | } else if (tersus_.position_type == "L1_FLOAT" || tersus_.position_type == "NARROW_FLOAT" || tersus_.position_type == "WIDE_FLOAT") { 221 | 222 | rtk_msg_out.fix_type.fix_type = mrs_msgs::RtkFixType::RTK_FLOAT; 223 | 224 | } else if (tersus_.position_type == "PSRDIFF") { 225 | 226 | rtk_msg_out.fix_type.fix_type = mrs_msgs::RtkFixType::DGPS; 227 | 228 | } else if (tersus_.position_type == "SINGLE") { 229 | 230 | rtk_msg_out.fix_type.fix_type = mrs_msgs::RtkFixType::SPS; 231 | 232 | } else if (tersus_.position_type == "NONE") { 233 | 234 | rtk_msg_out.fix_type.fix_type = mrs_msgs::RtkFixType::NO_FIX; 235 | 236 | } else { 237 | 238 | rtk_msg_out.fix_type.fix_type = mrs_msgs::RtkFixType::UNKNOWN; 239 | } 240 | } 241 | 242 | try { 243 | rtk_publisher_.publish(mrs_msgs::RtkGpsConstPtr(new mrs_msgs::RtkGps(rtk_msg_out))); 244 | ROS_INFO_ONCE("[RtkRepublisher]: publishing RTK from Tersus msgs."); 245 | } 246 | catch (...) { 247 | ROS_ERROR("[RtkRepublisher]: exception caught during publishing topic %s.", rtk_publisher_.getTopic().c_str()); 248 | } 249 | } 250 | 251 | //} 252 | 253 | /* emulateJump() //{ */ 254 | 255 | bool RtkRepublisher::emulateJump([[maybe_unused]] std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { 256 | 257 | if (jump_offset_ != 2.0) { 258 | jump_offset_ = 2.0; 259 | jump_hdg_offset_ = 1.0; 260 | fix_type_.fix_type = mrs_msgs::RtkFixType::SPS; 261 | } else { 262 | jump_offset_ = 0.0; 263 | jump_hdg_offset_ = 0.0; 264 | fix_type_.fix_type = mrs_msgs::RtkFixType::RTK_FIX; 265 | } 266 | 267 | ROS_INFO("[RtkRepublisher]: emulated jump: %f", jump_offset_); 268 | 269 | res.message = "yep"; 270 | res.success = true; 271 | 272 | return true; 273 | } 274 | 275 | //} 276 | 277 | /* toggleRandomJumps() //{ */ 278 | 279 | bool RtkRepublisher::toggleRandomJumps([[maybe_unused]] std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { 280 | 281 | if (!add_random_jumps_) { 282 | add_random_jumps_ = true; 283 | until_next_jump_ = 3 * _rate_; 284 | until_jump_end_ = 5 * _rate_; 285 | jump_amplitude_ = 1.0; 286 | } else { 287 | add_random_jumps_ = false; 288 | } 289 | 290 | ROS_INFO("[Rtk_republisher]: random jumps: %s", btoa(add_random_jumps_)); 291 | 292 | res.message = "yep"; 293 | res.success = true; 294 | 295 | return true; 296 | } 297 | 298 | //} 299 | 300 | // -------------------------------------------------------------- 301 | // | timers | 302 | // -------------------------------------------------------------- 303 | 304 | /* timerMain() //{ */ 305 | 306 | void RtkRepublisher::timerMain(const ros::TimerEvent &event) { 307 | 308 | if (!is_initialized_) 309 | return; 310 | 311 | mrs_lib::Routine profiler_routine = profiler_->createRoutine("timerMain", _rate_, 0.01, event); 312 | 313 | mrs_msgs::RtkGps rtk_msg_out; 314 | 315 | if (!got_odom_) { 316 | 317 | return; 318 | } 319 | 320 | rtk_msg_out.header.stamp = ros::Time::now(); 321 | rtk_msg_out.header.frame_id = "utm"; 322 | 323 | // copy the position, orientation and velocity 324 | { 325 | std::scoped_lock lock(mutex_odom_); 326 | 327 | rtk_msg_out.pose = odom_.pose; 328 | rtk_msg_out.twist = odom_.twist; 329 | 330 | pose_msg_out_.pose = odom_.pose; 331 | pose_msg_out_.pose.pose.position.x += jump_offset_; 332 | pose_msg_out_.pose.pose.position.y += jump_offset_; 333 | 334 | double hdg = mrs_lib::AttitudeConverter(pose_msg_out_.pose.pose.orientation).getHeading(); 335 | 336 | try { 337 | pose_msg_out_.pose.pose.orientation = mrs_lib::AttitudeConverter(pose_msg_out_.pose.pose.orientation).setHeading(hdg + jump_hdg_offset_); 338 | } catch (...) { 339 | ROS_ERROR("[RtkRepublisher]: %s:%d, could not set heading", __FILE__, __LINE__); 340 | pose_msg_out_.pose.pose.orientation = mrs_lib::AttitudeConverter(pose_msg_out_.pose.pose.orientation); 341 | } 342 | 343 | pose_msg_out_.header.stamp = ros::Time::now(); 344 | pose_msg_out_.header.frame_id = "local_origin"; 345 | } 346 | 347 | rtk_msg_out.pose.pose.position.x += _offset_x_; 348 | rtk_msg_out.pose.pose.position.y += _offset_y_; 349 | 350 | rtk_msg_out.pose.pose.position.x += jump_offset_; 351 | rtk_msg_out.pose.pose.position.y += jump_offset_; 352 | 353 | rtk_msg_out.pose.pose.position.x += random_jump_; 354 | rtk_msg_out.pose.pose.position.y += random_jump_; 355 | 356 | rtk_msg_out.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; 357 | rtk_msg_out.fix_type = fix_type_; 358 | 359 | // set orientation and twist to zero to unify the data provided by physical and simulated RTK 360 | rtk_msg_out.pose.pose.orientation.x = 0; 361 | rtk_msg_out.pose.pose.orientation.y = 0; 362 | rtk_msg_out.pose.pose.orientation.z = 0; 363 | rtk_msg_out.pose.pose.orientation.w = 1; 364 | 365 | rtk_msg_out.twist.twist.linear.x = 0; 366 | rtk_msg_out.twist.twist.linear.y = 0; 367 | rtk_msg_out.twist.twist.linear.z = 0; 368 | 369 | rtk_msg_out.twist.twist.angular.x = 0; 370 | rtk_msg_out.twist.twist.angular.y = 0; 371 | rtk_msg_out.twist.twist.angular.z = 0; 372 | 373 | if (add_random_jumps_) { 374 | if (!random_jump_active_ && --until_next_jump_ <= 0) { 375 | random_jump_ = jump_amplitude_; 376 | random_jump_active_ = true; 377 | ROS_INFO("[RtkRepublisher]: jump %.2f added to RTK", random_jump_); 378 | } 379 | 380 | if (random_jump_active_ && --until_jump_end_ <= 0) { 381 | random_jump_ = 0.0; 382 | random_jump_active_ = false; 383 | until_next_jump_ = std::floor((double)std::rand() / RAND_MAX * 20.0 * _rate_); 384 | until_jump_end_ = std::floor((double)std::rand() / RAND_MAX * 10.0 * _rate_); 385 | jump_amplitude_ = std::floor((double)std::rand() / RAND_MAX * 5.0); 386 | ROS_INFO("[RtkRepublisher]: RTK jump ended. Next jump after %d samples, %d samples long, %.2f amplitude", until_next_jump_, until_jump_end_, 387 | jump_amplitude_); 388 | } 389 | } 390 | 391 | try { 392 | rtk_publisher_.publish(mrs_msgs::RtkGpsConstPtr(new mrs_msgs::RtkGps(rtk_msg_out))); 393 | ROS_INFO_ONCE("[RtkRepublisher]: publishing RTK from Gazebo simulator."); 394 | } 395 | catch (...) { 396 | ROS_ERROR("[RtkRepublisher]: exception caught during publishing topic %s.", rtk_publisher_.getTopic().c_str()); 397 | } 398 | 399 | try { 400 | pose_publisher_.publish(geometry_msgs::PoseWithCovarianceStampedConstPtr(new geometry_msgs::PoseWithCovarianceStamped(pose_msg_out_))); 401 | ROS_INFO_ONCE("[RtkRepublisher]: publishing RTK from Gazebo simulator."); 402 | } 403 | catch (...) { 404 | ROS_ERROR("[RtkRepublisher]: exception caught during publishing topic %s.", pose_publisher_.getTopic().c_str()); 405 | } 406 | } 407 | 408 | //} 409 | 410 | } // namespace mrs_uav_odometry 411 | 412 | #include 413 | PLUGINLIB_EXPORT_CLASS(mrs_uav_odometry::RtkRepublisher, nodelet::Nodelet) 414 | --------------------------------------------------------------------------------